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I. INFRODUCTION 


A. BACKGROUND AND LITERATURE SURVEY 

Robots are presently an integral part of industrial processes. They perform tasks with 
high precision, speed and reliability. These same features make robots attractive with 
regards to space applications. 

Space based robotics platforms experience conditions unlike those of their terrestrial 
counterparts. With respect to the dynamics of the systems, the most notable difference is 
the absence of a fixed base on which to locate the manipulators. The consequence of the 
difference is that the motion of the space based manipulator transmits forces and moments 
to its mounting base resulting in translation and rotation of the base itself. This of course 
impacts the location of the manipulator’s end effector. The problem is further complicated 
in that the disturbances are not simply a function of the present manipulator joint angles 
but are also a function of the joint angle histones preceding the current configuration. 
(Ref. 1) 

A number of approaches have been used for dealing with this coupling of joint angle 
histories and spacecraft main body attitude. Wang (Ref. 2) eliminates the problem by 
carefully defining what he expects of his dual-arm maneuverable space robot. He preposi- 
tions the manipulators such that they are configured to grasp the payload once the vehicle 
moves within range. After the manipulators are in position, their joints are locked while 
the spacecraft maneuvers to a location and attitude near the payload. Next, the vehicle 
approaches the payload in a straight line until the end effectors can grasp the payload. 
While the manipulator joints remain locked, the vehicle repositions the entire rigid body 


system to the desired payload destination. At this point, the payload 1s released and the 


vehicle backs away along a straight line. The repositioning of the payload 1s accom- 
plished by means of the vehicle attitude control rather than altering the joint angles in the 
manipulators. The manipulators themselves are not moved except when the attitude dis- 
turbance they impart to the vehicle is of no importance. Maintaining vehicle attitude dur- 
ing manipulator motion is not a requirement. 

Longman, Lindberg and Zedd (Ref. 3) calculate the disturbance torques caused by 
manipulator motion. This information is used to calculate reaction wheel commands 
which will compensate for the disturbance torques. In this way, spacecraft attitude control 
is maintained while the manipulator 1s repositioned. 

If the vehicle does not contain reaction wheels, the primary source of attitude control 
1s probably reaction control thrusters. Because fuel 1s consumable and hence mission lim- 
iting, firing thrusters to hold spacecraft attitude should be avoided whenever possible. 
Vafa and Dubowsky (Ref. 4) and Longman (Ref. 5) use similar approaches to eliminate 
the need for reaction thruster firings. Both techniques involve constructing a manipulator 
trajectory which involves revolving the manipulator in a small coning motion at interme- 
diate stages of the payload repositioning maneuver. This motion imparts a slow rotation 
of the spacecraft about the coning axis. Careful use of the coning locations permits repo- 
sitioning of the payload between any arbitrary locations (within manipulator reach) and 
attitudes while also changing the spacecraft to any desired attitude without the need for 
thruster firings. This technique does not, however, maintain a particular spacecraft atti- 
tude during the maneuver. 

Nakamura and Mukherjee (Ref. 6) use a technique called the bi-directional approach. 
This method represents a six degree of freedom (DOF) manipulator mounted on a space 
vehicle as a nine variable system (six Joint angles and three spacecraft attitude angles) 
with six inputs (the manipulator joint torques). They attack the problem from both ends 


by integrating forward from the initial conditions and backwards from the desired final 


conditions. A Lyapunov function guarantees that the two solutions will converge at some 
intermediate time during the maneuver. As in Ref. 4 and Ref. 5, the payload is reposi- 
tioned to the desired location and attitude and the attitude of the spacecraft main body is 
changed to its desired onentation. However, the main body attitude during the maneuver 
is not controlled. Furthermore, the joint angle trajectories calculated by this method con- 
tain rapid, large oscillations near the maneuver start and stop times and noncontinuous 
derivatives at the convergence time. 

Vafa (Ref. 7) succeeds in using a single space-based manipulator to control spacecraft 
attitude dunng a repositioning maneuver. He does this by employing a nine DOF manipu- 
lator. This manipulator has enough redundancy in its kinematics to control the end effec- 
tor location and attitude and the spacecraft attitude. Six DOF are allocated to 
repositioning the payload and the remaining three DOF are used to control the spacecraft 
attitude. 

Like Ref. 7, the primary objective of Chung, Desa and deSilva (Ref. 8) is to address 
the disturbances transmitted to the spacecraft by the manipulator motion. They also use a 
single manipulator with redundant kinematics. Because they use inverse kinematics to 
find the joint torques, the manipulator redundancy prevents the existence of a unique solu- 
tion. A solution is selected from among the infinity of possible solutions by means of 
minimizing a cost function of the magnitudes of the forces and torques transmitted to the 
base. 

Torres and Dubowsky (Ref. 9) also focus on the spacecraft attitude disturbances 
caused by manipulator motion. They recognize that for any given point in joint space, 
there is a direction of motion which produces minimum spacecraft attitude disturbance 
and a perpendicular direction of motion which produces maximum spacecraft attitude dis- 
turbance. A tool called the enhanced disturbance map (EDM) depicts these directions 


graphically. The EDM permits users to plan manipulator trajectories that lie on or near 


zero disturbance paths. For non redundant manipulators, this may involve repositioning 
the spacecraft itself prior to the manipulator repositioning maneuver. If the manipulator 
has redundant kinematics, one can find a zero disturbance path to connect the manipulator 
trajectory endpoints without having to reposition the spacecraft initially. This technique 
considers only the location of the manipulator endpoint and not its attitude. 

Configurations with multiple manipulators have also been explored. The closed chain 
nature of these configurations prevent the use of some of the techniques already discussed. 
In addition, controlling multiple manipulators raises the issue of cooperation between the 
manipulators. 

Nguyen, Pooran and Premack (Ref. 10) develop a PD controller for a fixed base, two 
DOF, closed chain manipulator system. The system is linearized by means of Taylor 
series expansion about a point designated as the robot’s “home” point. Pole placement is 
then used to select controller gains. 

Hu and Goldenberg (Ref. 11) derive an adaptive control scheme for multiple nonre- 
dundant manipulators mounted to a fixed base. In this reference, coordinated control 
involves controlling the motion of the grasped object, the contact forces between the 
object and its environment, and the internal forces within the object caused from being 
held by more than one manipulator. 

For a space based system, contact forces between the payload and its environment are 
less likely to be important. Walker, Kim and Dionise (Ref. 12) present just such an adap- 
tive controller. 

Coordinated control of multiple manipulators assumes a different meaning according 
to Yoshida, Kurazume and Umetani (Ref. 13). Although they propose a system with two 
manipulators, only one actually grasps the payload. The other is used to provide counter- 


balancing torques to the spacecraft main body. The role of the second manipulator is sim- 


ilar to a reaction wheel in that its primary function is to control spacecraft attitude rather 
than reposition a payload. 

Ahmad and Zribi (Ref. 14) apply a Lyapunov controller to a fixed base, multiple 
manipulator system. Asin Ref. 12, they are concerned with controlling the payload posi- 
tion and its internal forces. To do so, the method requires sensors to measure the forces 
and moments created by each manipulator. They also present an adaptive version to con- 
trol this system. 

While still addressing payload position and internal forces, Schneider and Cannon 
(Ref. 15) use a technique called object impedance control to achieve coordinated control 
among the manipulators. This method views the payload as being anchored to a desired 


location by a spring/damper system. 


B. DISSERTATION OVERVIEW AND OBJECTIVES 

This research is concerned with the cooperative control of a space based manipulator 
system with multiple manipulators handling a common payload. The scope is limited to 
planar motion in which the spacecraft is allowed to rotate but not to translate. These 
restrictions permit experimental verification in the Spacecraft Dynamics and Control Lab- 
oratory at the Naval Postgraduate School. The objectives of this research are to 1) develop 
a stable control law which facilitates cooperation among the manipulators as they reposi- 
tion the payload, 2) minimize joint actuator effort, 3) reduce the disturbance torque trans- 
mitted to the spacecraft main body by the manipulator motion, and 4) validate the 
analytical development with experimental results. 

Chapter II develops the analytical model in detail. Coordinate systems are defined and 
the equations of motion are derived. A technique for finding control torques which mini- 


mizes a weighted norm is presented. A globally stable control law is developed using 


Lyapunov methods. The idea of using a reference trajectory to describe the motion is 
applied as are methods for choosing the reference trajectory. 

Chapter III verifies the analytical model with several test cases. The model is evalu- 
ated for compliance with the principles of conservation of kinetic energy and angular 
momentum. After establishing the validity of the model, results from simulations are pre- 
sented. The stability of the controller is illustrated as is the dramatic improvement In per- 
formance when a reference trajectory is included. Results from a simplified control law 
which is more practical to implement are included and compared to the complete control 
law version. 

Discussion of the experimental work is contained in Chapter [V. This chapter includes 
a description of the experimental setup. As might be expected, actual hardware demon- 
strated that there are differences between the ideal world of the analytical model and the 
real world of hardware implementation. 

The summary and concluding remarks are presented in Chapter V. This chapter also 


contains suggestions for future work in this field. 


H. ANALYTICAL MODEL 


The analytical model represents a spacecraft with two manipulators attached. The 
manipulators have already firmly grasped an object hereafter referred to as the payload. 
The manipulators are about to reposition the payload with respect to the spacecraft. The 
ensuing dynamics between the spacecraft, manipulators, and payload are the topic of this 
research. What occurs before the manipulators grasp the payload and after they release it 
is beyond the scope of this investigation. The scope is narrowed further by confining the 
motion to be two dimensional and allowing the spacecraft to rotate but not translate. 
These assumptions are consistent with hardware restrictions during experimental verifica- 


tion. 


A. COORDINATE SYSTEMS 

Figure 1 shows a schematic of the overall system. This diagram illustrates the rela- 
tionship between the coordinate frames used to develop the equations of motion. All 
angles are measured positive counterclockwise. The centerbody, manipulator links, and 
payload are assumed to be rigid bodies. Therefore, member lengths (4 |, 4 >, 42), 49, and 
tp), distances to member centers of mass (4.9, 4) 1, 41.2, er}, &p2, and 4p), and the loca- 
tion of the left and right shoulders (4 9, 9; 9, 49, and 9),9) remain constant. An inertial 
axis system is located on the centerbody at the point of rotation. A body fixed coordinate 
frame uses the same origin as the inertial frame but rotates with the spacecraft centerbody. 
The x-axis of this frame points to the centerbody center of mass. The centerbody attitude, 
Qo, is the angle between the inertial x-axis and the spacecraft centerbody x-axis. Each 


manipulator link has its own set of body axes. A coordinate frame attached to the left 
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Figure 1: Dual Two-Link Manipulator Configuration 


shoulder aligns its x-axis along the longitudinal axis of manipulator Link L1. The attitude 
of this link, 6) ,;, is zero when the link lies on a ray extending from the inertial origin 
through the left shoulder. The attitude of Link L2 1s defined by a coordinate frame 
attached to the left elbow. The attitude of this link, @; 5, 1s zero when the link is parallel 
with the proceeding link, Link L1. Similar coordinate frames and definitions apply to the 
right manipulator. The payload attitude, 0), 1s referenced to the inertial frame. The Carte- 
sian coordinates of the payload center of mass are also with respect to the inertial frame. 
A set of generalized coordinates which describe the system include the centerbody atti- 
tude, joint angles for all of the manipulator links, and payload attitude and position. 


if 
) 


1 = 6, 81 Pho Opi Pree Op Xp Y) (1) 


Six joint actuators apply torques at the shoulder, elbow, and wnist of each manipulator. 
A reaction wheel applies a torque to the centerbody. The actuators can be grouped into a 


control vector 


T 
us i PhS be bye oR Sa RE tae (2) 


where the first element is the reaction wheel torque. The remaining elements are joint 
actuator torques. The first letter of their torque subscripts indicates left or right arm. The 


second subscript indicates shoulder (S), elbow (E) or wrist (W). 


B. EQUATIONS OF MOTION 
The equations of motion for this system are developed using Lagrange’s equations for 
a dynamic system with holonomic constraints. 
all 
oe Oka a (3) 
dt\aq) eq 


subject to constraints 


Aq+A, = 0 (4) 


where L=T-V 
T is kinetic energy 
V is potential energy 
q is the generalized coordinates vector 
q is the generalized velocities vector 
Q is the vector of applied nonconservative forces 
al X are the constraint forces 
Beginning with Lagrange’s equation, the equations of motion can be rearranged into 
an alternate form. The inertia matrix, M, is a function of the generalized coordinates. 
Since the potential energy is a function of only the generalized coordinates, the partial of 
the Lagrangian with respect to the generalized velocities does not contain any potential 


energy terms. 


cia Mig (5) 


Cq 


Differentiating Eq. (5) with respect to time leads to 


digi ole .  .TdM : OM 
sige] = Mita’ Ge = Mae Fi caue 6 
U\ Og eats 


Replacing the Lagrangian with expressions for kinetic energy and potential energy 


results in 
OL 1/.1eM OV 
Fa (q 4) ae 7) 
Og ne eq Cq 
Eq. (6) and Eq. (7) can be substituted back into Eq. (3) to produce 
Ma+—(q7Sn 4) + = Q4aln (8) 
Oq~ 6q ~ - 
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The second term on the left-hand side of Eq. (8) contains the centripetal and Coriolis 


torques. Replacing this with the G matrix leads to 


OV 
Mg +G(q,d) + = QtAA (9) 
: he aq : 
After substituting the matrix form of the generalized forces into the equations of 


motion, one has 
OV 
Mj+G+— = Bu+A‘a (10) 
: eq : 


The following sections develop expressions for the inertia matnx, Coriolis and cen- 
tripetal accelerations, generalized forces, and constraints imposed by the closed chain 
geometry of the system. 

1. Inertia Matrix, M 

The inertia matrix 1s found by calculating the system kinetic energy and expressing 


it in the form 


T = 54" [M(qyle (11) 


The inertia matrix is the term bracketed by the generalized velocity vectors. The 


total system kinetic energy 1s the sum of the kinetic energy of all the pieces. 
i Wate eo pa poe p (12) 
Kinetic energy of individual components can be found from 


ea Nad 
T, = Pie eat (Ea) (13) 


I; is the member moment of inertia about its center of mass 
@; 1s the angular velocity 
m, is the mass 


f 1s the inertial velocity of the center of mass 
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The centerbody angular rate and center of mass position vector are given by 


oO, = or (14) 


0 


[ 


= bak (15) 


( c0' 


Differentiating Eq. (15) results in the velocity of the centerbody center of mass 


i iz £6 Yo (16) 


Substituting Eq. (14) and Eq. (16) back into the expression for kinetic energy (Eq. 


(13)) and collecting on the angular rate term leads to 


0 Cscy 


] eee 
T, = coca C06 (17) 


Similar developments are used for each of the remaining pieces in the system. For 
the left manipulator link between the shoulder and elbow (Link L1), the angular velocity is 
a combination of centerbody rotation and rotation of the link with respect to the center- 


body. 


w 


, = O,+6) | (18) 


L 


The position vector to the center of mass 15s 


Spa (19) 


Gane £ cos te 


r Xo + €,sinO 


LO aaa 

The first two terms in the position vector represent the location of the left shoulder. 
Differentiating the position vector gives the expression for the velocity vector. Because 
none of the coordinate axes used in the position vector expression are inertial, their rota- 


tion must be included as well. 


ae £6089) p¥o-G Sin, gXo +4 


Sane (20) 


After Eqs. (18) and (20) are substituted into Eq. (13) and terms are grouped by 


angular rates, the kinetic energy 1s 


52 2 2 
Ua 89 (0.5 (1, , + my ecu +m, 40) (21) 
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+m, Sof iy (sinO, gsi (0 Oi) a cos@, cos (Or, Oe) ) 


ie! 


Aes 
Ooen een.) 


Opn, (l, mite Ce +i, 6 9%) 1 (sin®, ,sin (8, 5 +9, ,) + cos), ,cos (0, 9 +0, ,))) 
The angular rate for the left forearm includes the centerbody angular rate as well as 


the angular rates of the body axes on Links L1 and L2. 


0, = 69+6,,+6,9 (22) 


This link’s center of mass position vector Is 


Fo = 460891 oho +4 g Sin, SatG 8 +E 08), (23) 


_- 


Differentiating the position vector gives the velocity vector. 
= & 9, ¢089, p¥o-4 p, sin, go t+G | Oy, + 12.2%) | (24) 


The kinetic energy expression for Link L2 is found after substituting Eqs. (22) and 


(24) into Eq. (13) and collecting terms with common angular rates. 


2 p 5 
os Poet tim t +m, 56, =i orn) (25) 


m, 94 of ; (sin8, sin (8, , +9, ,) + cosO, ,cos (0, +8, .)) +m,6 .f, ,cos@, , 


+ mM, 94 ofp 2 (sin 8, sin ce, Se tO) + cos, cos (0 sean +0, 5)) ) 


LO 


w 2. 
+0, , (0.5 (I, + eG +m) 5f) 5} + m, 54 14) 20089, 5) 


ae 
+ 0.5619 (1, 5+ m, ,£, >) 


7 2 2 
+8581). +m, 94, +m, 4,24 SM AGN pCO 


+m, 544, (sin, ,sin (6, , +9, ,) +cosO, ,cos (Ona 


Coat ( sin@, ,sin on ae ey, + cos8, cos co; 10 + 0,,) )) 


e ° 2 
+ 8811, 5 +m, 4) 2+ 24 14120089) > 


+m, 54 94) > (sin®, sin Ore tee: ey, +cos0, pcos (8, eae ie EON) ) 
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+0; 9, 2(1, 5 >+ ree £2 FG 1&1 20088; 5) 
The development for the nght manipulator kinetic energy parallels that for the left 


manipulator. For the upper arm portion (Link R1), the angular rate is 
On) = 85+ 9p] (26) 
The position vector is constructed by finding the coordinates of the night shoulder 


and adding the vector from the shoulder to the center of mass. 


es £,£088p 9X0 + & SiO RGVo +t Cpik,, (27) 
The time rate of change of the position vector 1s 


i = £99 F088p (V0 Gy SiN Op do + Lp 


(28) 


oe 
After calculating the kinetic energy for Link R1 and grouping terms with common 


angular rates, the resulting expression is 


; a2 
T,, = 09 (05U,, 4m, fe monta.) (29) 


fat Cote my (sin, ,sin (Cpt Oo + cos@, cos (8... +8, ,)) ) 


ae 
+0.50p, (I, Eee ciee) 


‘Sebhe 2 : : 
+O08p; Tg, + Mp OR + Mp AeoEp, (sin Opysin (O25 +O, ,) + cos8,,cos (0,5+92,))) 


Angular rate of the right forearm 1s 


Op, = 99 +9R1 + OR2 (30) 


Its position vector 1s 


oe 6. 960S9p 9X0 + 4.5819, Vo + faiX | 


VoGas (31) 


Differentiating Eq. (31) produces the velocity vector for Link R2 center of mass. 


a = R0° SOR oV0~ .9@,)sinO, oko Ae OR ,, +L R22 p, (32) 
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The kinetic energy resulting from substituting Eqs. (30) and (32) into Eq. (13) and 


collecting common angular rate terms is 


; 2 2 2 
Tp = Gio (ipetin, 0 shine tet nto) (33) 


+m (sinO, pitt (O,, +9, ,) + c0s0,, cos (0 JPe arti tad cos6,., 


R2 feof Rl Rot R2 Ki eR2 


+ Mofo R? ( si, 9 sin Lone +02, +9)5) + cos 0, ,cos (G8. +02, +. 0.5)) ) 


aoe 2 
+ 6p, (0.5 (1, aera ra ante gle) + mp 5f0 14 p71 205925) 


2 2 
UU g edie ita stce.) 
oo 5 
+ 0989p; (Ip, +mp lo, ae Oot Mince ten c ON ns 
+mp5f RO fe (sinO, goin (07, +057) + cos, COS (Oona t Pe 


Loe (sinO, 9 Sit (0 


Mpo4poK Ro , +9 ao) + cos), pcos (0, yee ue 


Roe 
+ 698p5 (I,5 Dh eee ite te  COSUL. 

+p 5404 eu sin, 9 sin (Oo get say an +0.) + cosQ), cos ne Oar a0) ) ) 
Beegen (lest ties. PAM te ta cOSe 5) 


Expressions for the payload are considerably simpler because inertial coordinates 


are available. The angular rate is 
O, = Op (34) 
It is not necessary to describe the payload center of mass by passing through either 


shoulder as was the case with the manipulator links. The position vector is 
r= XpN.+Y,Ny (35) 
The velocity vector is also simpler because of the inertial frame. 


a = XpN, te YpN, (36) 


(5 


The payload kinetic energy is derived from substituting Eqs. (14) and (16) into Eq. 
C13): 


] ie 2 ene 
(i (1,6) +m, p+ ¥p)) (37) 


ns 


After substituting the expressions for kinetic energy from Eqs. (17), (21), (25), 
(29), (33) and (37) into Eq. (12) and expressing the result in the matrix form of Eq. (3), the 
inertia matrix, M, 1s given by Eq. (38). Because the generalized coordinates for the pay- 
load are referenced to an inertial coordinate frame, the inertia matnx 1s decoupled between 
the payload and the rest of the system. Coupling does exist between the spacecraft center- 


body and each of the manipulators. 


| 
M1) Mi. Mj3 Mig Mis 00 9 
M,, M,, M,, 0 0 0 0 O 
M,, M,, M,, 0O- 0 05.0 0 
M QO OFS NiO eee 
Nee 4] 44 "45 (38) 
M,, OF 0 M., M.. 0 0 O 
O> 20) 5 Oe ee) I, 0 0 
OO HO Oi, 
nO 60UlClC OO 0 Oo my 
Expressions for the individual elements in the inertia matrix are given by 
Mes = Ic ean, ee (39) 
55.7 aie Inne 
M,, = M,, = M, 5+ Mp rfp hcp ,cos8, (40) 
M,< = M,, = M, 5 + Mp nba fc, 5 COS (Op, +@,,) (41) 
fe 2 
Mya = Mys +1ay + Moy fp 2 0080p + Mp AR) +MpoéR (42) 
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M,, = My, = M,, +4.) (m, 4c + Mp 54,1) COSOp | (43) 


14 K | 
+ Mp ofp glCp> cos (0), | +0),.5) 
M33 = +m, ec; 4 (44) 
M.,, = M,, = M,,+m, 94 ,éc, ,cos0, , (45) 
M,, = M,, = M, 3 +m, 46 ofc, » cos (0, , +9, 5) (46) 
M,, = M,,+1,, +m, 56 ,fc, ,cos0, +m, jt, +m, of (47) 
M,, = M,, = M,,+4,(m, ,&, , +m, 54 ,) cos0, , (48) 


+ m, »f gfe) COS (0, , + 0, 5) 


= 
I 


2 2 2 
I, +M,.+M,, +m fc, + Cn es m, )¢ Lot (Mp, + Mp5) é ro (49) 


+20, (mp fp | ) cosO,, + 2m f fc, 27 008 (0, , +9 


Mig fi R2 RO R2) 


2L (m, ,f¢, | + m, 54 1) cosQ, , + 2m, 54, fC) 4 COS (9, | +0») 


. Centripetal and Coriolis Matrix, G 


The G matrix contains all of the centripetal and Conolis terms. It 1s most easily 


found using 


WO. 


he 
(Qe: 
G(q.q) = |9 4 C q (50) 


Res) 
eC 


where the elements of C\ are defined by the Chnstoffel symbol 


| OM.. OM. OM. 
cl) = eet ee a (51) 


AZ 


The form of the G matrix for the system of Figure | is given as 


(ope: 


ho 


@D 
II 
qa on 
— a 


C) 
Wn 


2) (Ss 


2 — 
G, = -& , (911 +2998 1) (m, 4, , +m 


ysind) , 


1241 


—m, 54 ofc, 5 (289 (91 | +6, 5) + (8; ,+9;5) ) sin (Gi oy, 


wD a 
Ss tle +20)0p1) (m, , 4c, , pl oe) sinQ, , 


—m, 5& , fe, 9p? (28 + 20, ; +Op3) sin On. 


Sey anh Pine (285 (Op, + Op) a (Gna GES) a) sin (O,, +0,5) 


G, = & ,89(m, ,&, , +m ,) sin8, »-m, 54 ,£c, 812 (289 + 26; | + 0,2) sind, , 


24 | a 


co a 
+ m, 44 ofc; 80 sin (9, | + 0») 


Wy ato oe 
G, =m, 54 1) (911 +912) sin6,, +m, 44 fc, ,8osin (9, , +9, 5) 


Gy = 4580 (mp en, + Mp 741) SINO,, — Mpr4e Cp Op2 (200 + 20K + OR?) sin®, 5 


Ob 
+n, ofp 9£Cp 5% sin (8, + O25) 


: : 2s wid 
G, = Mp 9b {p> (Ori + Op >) sin0, 5 + Mp54,fc, .Osin (9, , aN) 
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(52) 


(53) 


(54) 


(55) 


(56) 


(57) 


3. Generalized Forces, Q 
The generalized forces are found using the principle of virtual work. When there 
is no reaction wheel on the centerbody, the system does not experience any external forces 
capable of performing work. Six joint actuators apply torques at the shoulder, elbow, and 


wrist of each manipulator. 


T 
6 Us MLE “LW URS “RE am (58) 


Uu = 
Ug is simply the joint actuator subset of the complete actuator torque vector, u. The 
total virtual work is the sum of the torques applied to the individual bodies times their vir- 


tual angular displacements. 


N N 
SW = > SW. = > (M50, (59) 
= r= 1 
When the left shoulder joint actuator applies a positive torque on Link LI, a nega- 


tive torque is also applied to the centerbody. The virtual work performed by the left shoul- 


der motor is 
OW) < =U¢ (58, +60, | - 68) (60) 


where the positive angles are those associated with the change in Link L1 attitude and the 
negative angle 1s associated with the change in centerbody attitude. The left elbow actua- 
tor makes a positive contribution to Link L2 attitude and a negative contribution to Link 
L] attitude. 


S5W, ,. = u, ,, (50, +60, , +60, ,-50,-80, ,) = u, ,,50 (61) 


2 ies De 
The joint actuator at the left wrist makes a positive change in the payload attitude 


and a negative change in Link L2. 


SW, w = Uy (50, - 50, - 58, , - 88, 5) (62) 
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The nght shoulder actuator makes a positive contribution to Link R1 attitude and a 


negative contnbution to centerbody attitude. 


BWpo = Upc (58, +40), — 605) = Up OO), (63) 


RS 
Link R2 has a positive virtual displacement due to a positive torque at the nght 


elbow. The same torque causes a negative virtual displacement of Link R1. 


SWay = Up, (50) +50, +50,, - 50, - 50,1) = Up, dO, (64) 


The right wrist actuator has a positive influence on the payload and a negative 


influence on Link R2. 


SWry = Upw (50, - 50, - 56, , - 50,,) (65) 


RW 


Gathering Eqs. (60)-(65) together produces 
OW = (— Uy wr Upw) 89 + (Uy g — Uy w) 98, + CU UL w) 8), (66) 


+ (Ugg — Uw) O9Q; + Upp — Uw) 8p, + (UL wo UR) O8p 
With respect to the system equations of motion, the genera!!zed force correspond- 
ing to a particular generalized coordinate is that portion of the virtual work associated with 


the same generalized coordinate. Now Eq. (66) can be transformed into a matrix form. 


Q. = Bou (67) 


6 


where B is the control influence matrix given by 


OOPS Ore 
OS E0200 
OF = OO) (0, 
OOPS ON JE 0 (68) 
U1 AO 0 
O70 Oem 
00 000 0 
OO TOS U ro s0 
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The only effect of a positive reaction wheel torque applied to the spacecraft is to 
alter the centerbody attitude in a positive direction. This manifests itself in the control 
influence matrix in the form of another column. This new column is all zeros except for a 
single one corresponding to the location of the reaction wheel torque in the u vector. With 
the reaction wheel torque as the first element in the control vector as in Eq. (2), the com- 


plete control influence matrix is 


OO) O01 
O10 SI Wo © 
Oona) 000 20 
0010 10an0 = 
ORO 209 Only =a 
0001001 
0000000 
(0800-00100), 


B = (69) 


4. Constraints Matrix, A 
Because the eighth order system under consideration has only four degrees of free- 
dom, an additional four equations are needed to describe the constraints. The eight gener- 
alized coordinates are not independent. The constraint equations embody the information 
that the manipulators are both grasping the payload forming a closed chain system. The 


constraints matrix is derived by wnting the system constraints in the Pfaffian form as 
AgtA, = 0 (70) 


These equations come from geometnic relationships of expressing the payload cen- 


ter of mass Cartesian coordinates in terms of the other generalized coordinates. 


£, cos (0, +8, ) +1, cos (0, srs oa) +, ,cos (GF +0) 5 + O15 +0, 4) + fc, cos0, =Xp 
 ,sin (8, +8, 5) +4 , sin (0, + 9, +9, ,) +4 .sin (0, +8, +8, , +0) 5) + cpsin®, = yp (71) 


£, cs (8, + 8,5) +4, 00s (8, +O, + Op 1) +4.,008 (0, + 929 + OR, + OQ5) — (4 - Lp) cosO, = Xp 


t, sin (0, +620) +, sin (0,+6,5+9,,) + @ sin (G7 en oe on) ~- (= lc,,) sin®, =Yp 
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To get the Pfaffian form of Eq. (70), differentiate Eq. (71) and rearrange terms. 


The result is 


05 

Bk 
A, A,A,, 9 0 A, -1 01/82 ; 
A, Ax, A; 0 0 M46 i) Bey = 0 
Aa, 08" 0) AGN eee Op. 0 
Q) 

Ne O- 0 A ei bp 

Xp 

aN 


(72) 


The constant term, Ao, is a zero vector. The individual element in the constraints matrix 


are given by the following equations 


Ai = —fc,,sin0, 


Ax6 = fc, cosO, 


A 4 = (£,-“c,,) sinO,, 


Aye == (4,—-4c,) cos@,, 


cos (Sh +0,..+ On| + 0,5) 


Ags = ae) RO 


Agg = Ags + & 1 008 (8) + Ong + Bp 1) 


BVP rye 3, COSC EET) 


Abas —t,,sin (9,+9,5+8p, +05) 


35 
Az4 = A357 £1 sin (8, + Ong + By) 


AS ak 


31 34 ~ Aap Sin (By + O29) 


A,3 = £ cos (9, +} 91 0 + oF | 4 Q, ») 
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(73) 
(74) 
(75) 
(76) 
(77) 
(78) 
(79) 
(80) 
(31) 
(82) 


(83) 


Aj, = Aj,+ fo , cos (0, +8, + 0. ) (84) 


m2 costo Lo) :) 
Ays = —& ,81n (8, +9) 9 +9) | +9) 5) (96) 
Ay = Ay3— 4, sin (8, +9) 9 +8) (37) 
Ary = Ayx~& ysin (Oy + 9) 9) (38) 


If the manipulators are mounted on a fixed platform rather than a rotating base, an 


additional constraint equation is included in the A matnx. The constraint is that 6, is con- 
stant and therefore 

6, = 0 (89) 

This constraint is augmented into the A matrix by adding a fifth row. The first ele- 


ment in the row is a one. The remaining seven elements are all zeros. 


C. SIMPLIFIED EQUATIONS OF MOTION 

The potential energy term is zero because motion is confined to the horizontal plane 
and the system is composed of ngid members. The inertia matrix, G matrix, B matrix, and 
constraints matrix can be found from the results of the previous sections. The remaining 
unknowns are the actuator torques and the Lagrange multipliers. By using the equations 
of motion and the Pfaffian form of the constraints, one can eliminate the Lagrange multi- 


pliers. The time derivative of Eq. (70) is 


Aq+ Aq = 0 (90) 


Solving Eq. (10) for q and substituting the result into Eq. (90) permits one to find an 


expression for the Lagrange multipliers. 
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= = = 
h = (AMA’) (AM (G-Bu) - Aq) (91) 
The inertia matrix is always a Square matnx with full rank and therefore invertible. To 
investigate the invertiblity of AM !A‘, begin by creating a 4x4 matrix out of the third, 


fifth, seventh, and eighth columns of the constraints matrix. 


Oe 
Ann sO Or 
ON) 


| 0 A,, 9 -1| 

Inspection of this submatrix reveals that all of the rows and columns are linearly inde- 
pendent even if Aj3 = A>3 and A35 = Ays. Therefore, the A matrix always has rank of 4. 
The 4x4 matrix product AM A’ will also always have rank of 4 and 1s therefore invert- 
ible. Eq. (91) can be substituted back into the equations of motion (Eq. (10)) leaving the 


actuator torques as the only unknowns. The resulting equations of motion in which the 


Lagrange multipliers have been removed and potential energy is zero are 


Mq+G = Bu (93) 
where 
= T Sie! -| - 
G=G-A (AM A’) (AM G-Aq) (94) 
: Bi en 
B - Gal (AM 'A') AM 1p (95) 


D. REFERENCE TORQUES 
Given a reference trajectory of the payload with known displacements, velocities and 
accelerations, one can use the simplified equations of motion (Eq. (93)) to solve for the 


actuator torques that will produce the reference trajectory. 
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eh az Da (96) 


Deisees ref 
The equations for specific elements in the matrices of Eq. (96) are the same as already 
presented. The subscript “ref” merely means that the displacement, velocity and accelera- 
tion terms are the values from the reference trajectory. 
In this study, the total number of actuators 1s more than the system degrees of freedom. 
This situation is caused by the geometric constraints of multiple manipulators handling a 
common object. As a result, there are an infinity of solutions for the reference torques. 


One method to select a specific solution is to establish a cost function. An obvious cost 


function is to minimize a weighted norm of the actuator torques. 


Leal 
a sg eee ral 


The problem now becomes one of minimizing the cost function (Eq. (97)) subject to 
the constraint that the reference equations of motion (Eq. (96)) are satisfied. Augmenting 


the cost function with the constraint by means of another Lagrange multiplier leads to 


| a T (a ze a : 
_ pee yer 1 Bre ep ref Tper Gre 2) 


The minimum of the augmented cost function is found by taking the gradient of Eq. 
(98) with respect to the reference torques and with respect to the Lagrange multiplier. 


Each of the gradients is set to zero. 


Sali 
Vy J=0= Wut Brot (99) 
V7 =O) = Bret op Mer Fer 7 Gret (100) 


Eqs. (99) and (100) are two equations in two unknowns (y, U op Eliminating y 


results in an expression for the reference actuator torques. 
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-j2T € Bie! J’ - 2 
u = WB (BrerWy, Brot) (Myo + Greg (101) 


= fel 

Although the matnx product eae is an 8x8 matrix, it is not invertible. A 
pseudo-inverse 1s needed because the system has only four degrees of freedom. There- 
fore, the matrix product Eee ase is rank deficient and has a rank of four at most. This 
expression for reference actuator torques minimizes the augmented cost function (Eq. 
(98)) at each instant in time. Although the value for the reaction wheel torque is calcu- 
lated, it is not minimized by this function. The reaction wheel torque profile is dictated by 
the disturbance torques transmitted to the centerbody as a result of manipulator and pay- 
load motion. For a given reference trajectory, an infinite variety of joint actuator torques 
can produce that trajectory. However, a given reference trajectory has only one reaction 
wheel torque profile that is common to all the infinity of joint torque combinations associ- 
ated with that trajectory. Equation (101) selects from among the infinity of joint actuator 
torques the one combination that minimizes the weighted norm cost function. Although 
the selection is limited to a single choice, Equation (101) also produces the correct reac- 


tion wheel torque for the given reference trajectory. 


E. LYAPUNOV CONTROLLER 

This material in this section is based on Ref. 16. The purpose of any control law is to 
provide system performance that satisfies a specification. As a bare minimum, the control 
law must keep the system stable. Because of the highly nonlinear nature of this spacecraft 
robotics system, most control laws simply do not apply. The motivation behind using 
Lyapunov methods is to develop a control law with guaranteed stability. Recall the equa- 


tions of motion of the manipulator system are 


Mq+G = Bu (102) 


Solving Eq. (102) for q results in 
q = M | (Bu-G) (103) 
Substituting Eqs. (94) and (95) back into Eq. (103) and grouping terms according to 
the form 
q = C,u+C,q+C, (104) 


leads to the following expressions 


: 2. a 
C,)=M {1-A'(AM A‘) AM }B (105) 


a ro 
C,=-MA'(AM'A') A (106) 


= = oom = 2 
C,=M{A'(AMA') AM -1}G (107) 


Similarly, the reference maneuver accelerations can be expressed as 


Sef — eee ee ae (108) 


where again the reference subscripts on the C matrices indicate that reference maneuver 
values need to be used in their calculation. Let error quantities between the actual vari- 


ables and their reference maneuver counterparts be defined by 


dq = G- dor (109) 
6g = 4-4, (110) 
og = 4-4 (111) 
Now define an error Lyapunov function as 
U = 0.5(5q- &q) + f (Sq) (112) 


where f(Sq) 20. Differentiating Eq. (112) results in 
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, ge Ci 
i 1 











Let 
f sof ar | 
F = u ae (114) 
- ~ 18(8q,) 6(8q,)  2(8q,) 
Then Eq. (114) can be rewritten as 
U = 5q- (8q+F) (115) 


Substituting Eq. (104) and Eq. (108) into Eq (111) and then Eq. (111) into Eq. (115) 
produces 
U = 5q- {(C,u ~ ©) Ue + (C.q —©5 dep + (C, = cw, +t (116) 


If one lets the quantity inside the brackets of Eq. (116) equal -K5q where K) is a 
positive definite matrix, then one is guaranteed that U < 0 and therefore the system will be 


stable in the Lyapunov sense. Solving Eq. (116) for command torques, u, leads to 
u = C,! {-K 84+ Cy up (Cyd-Cy Go -(Cy-C, Y-F} (AIT) 


C, 1s an 8x7 matrix so ci is its pseudo inverse. Equation (119) finds the torques that 
should be used rather than the reference torques. All that remains 1s to choose a function 


for f (Sq). One can choose 
l ; 
f(Sq) = 55q'Kp5q (118) 


where like K., K, is required to be positive definite. Selection of values for the gain 
matrices is beyond the scope of this work. The simulations included in the next chapter 
use diagonal matrices with uniform values simply as a matter of convenience. One might 


try to adapt the linear quadratic regulator (LQR) problem to find more optimal gains. 
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After substituting Eq. (118) into Eq. (114) and that result into Eq. (119), one obtains the 


final form of the Lyapunov controller. 


)-(C,-C, )-K,8q} (119) 


rel 3 ef 


u = Cl {-K8q+ Cy Wp (Cad Cy 
If the differences between the reference trajectory and the system dynamics are small, 
the Lyapunov controller approaches the form of a proportional plus derivative (PD) con- 


trol law. 


F, REFERENCE TRAJECTORIES 

The reference trajectories describe the nominal path that the system follows in moving 
from the initial conditions to the desired final conditions. One need only specify reference 
trajectones for as many generalized coordinates as there are degrees of freedom. In effect, 
the generalized coordinates can be divided into two sets. One set contains the minimum 
number of coordinates needed to completely descnbe the system. The second set contains 
all remaining coordinates, (redundant coordinates). The choice of which generalized 
coordinates to specify is entirely arbitrary. A reasonable choice includes the payload coor- 
dinates and centerbody attitude since the user will probably be especially interested in 
these generalized coordinates. The redundant coordinates are the four manipulator joint 
angles. Given reference trajectones for the minimum number of coordinates exist, the 
redundant generalized coordinates can be derived. This research assumes trajectories are 
available which define displacement, velocity and acceleration for the centerbody attitude, 
payload attitude, and payload center of mass coordinates (Xp, Yp, Op and 89). 

1. Calculating Redundant Coordinates 

Figure 2 illustrates the relevant geometrical relationships to find the joint angles of 


the left manipulator. Xp, Yp, Op and 0, are obtained from the reference trajectory. Point 


LS is the left shoulder joint. It has Cartesian coordinates given by 
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Figure 2: Deriving Left Manipulator Joint Angles 


LS, = cos 10, sr (120) 


LS 
y 


G gsieo, + ng) (121) 
Point Q is the joint between the manipulator end and the payload. The Cartesian 


coordinates of this point are 


2) 
od 
HT] 


X, — £pcos8, (122) 


< = eo cn, (123) 
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The distance between the left shoulder and Point Q Is given by 


LSQ = J (Q,.-LS.)*+ (OVers.)- (124) 


The inertial angle formed by the vector from LS to Q is 


OF -- LS, 
: (125) 


Os. 


The dimensions of the tnangle formed by the manipulator joints are known. Using 


the law of cosines, the interior angles at the shoulder and elbow can be found from 











Z <2 Be 
B, = acos iia a (126) 
a 24, ,LSQ 

iE +0 =O: 

ie Z 
B, = =| ve | (127) 


a) eal 
All that remains is to algebraically construct the manipulator joint angles from 


other angles as follows 
6, = B, +B, - (0, +9, |) (128) 
0,5 = f, + 180° (129) 
The development for the nght manipulator is similar. Its geometry is depicted in 
Figure 3. 
Point RS is the right shoulder joint with Cartesian coordinate 


RS, = £08 (ee) 


(130) 


Ro? 


RS) = £9 sin Ate) (131) 


Point P is the joint between the manipulator end and the payload. The Cartesian 
coordinates of this point are 


i 


x 


Xp+ (4,-4.5) cos0, (132) 
| oar (4,-¢,) sin6, (133) 


y 


The distance between the right shoulder and Point P is given by 
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Figure 3: Deriving Right Manipulator Joint Angles 





ae _ 2 a 2 
RSP (ioe IE 1S (134) 


The inertial angle formed by the vector from RS to P is 


ee RS. 
B, = atan ema (135) 


From the law of cosines, the interior angles at the shoulder and elbow are 


oer Ro 


24, RSP eo) 


B, = ea 
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Z 2 2 
Ca Ge RSP 
Rl ie 
B. = acos i = sil (137) 


? 
20 Ge 


The geometry in Figure 3 gives the manipulator joint angles based on the other 
angles. 

Oe hy a Ole) (138) 

O., = 180°-B, (139) 

Recall from the discussion of the Lyapunov controller that torques are calculated 

based not only on the generalized coordinates but their velocities and accelerations as 

well. The redundant coordinates have just been found, but the redundant coordinates 


velocities and accelerations must still be developed. 


Differentiating Eqs. (122) and (123) expresses the velocity of Point Q. 
Oe = Xp + Ope sind, (140) 


Qy Yp-Ope_cos0, (141) 


But the coordinates of Point Q can also be expressed in terms of left manipulator 
variables. 


OF = O cos cor +0, 9) + , cos (0, a0, +6 + ,cos co HON +053 +8, 5) (142) 


LI? 
Q, = £ ,sin (6, +6, .) +4 , sin (8, +9, , +9, ,) +4 ,sin (8, +0, 9 +0, , +8, 5) (143) 


Differentiate these equations and rearrange the terms in the form of 


Q SD re, D (144) 
Qy O12] 
where 
Pee sin 0) 70, te 4 si (0, 1074+ 0,1) (145) 
D, (1, 2) = —£ sin ee tee iy +0) 5) (146) 
D, (2,1) = 4 cos (0, +0, , +9, +9, ,) +4 ,c05(0, +9, +8, ,) (147) 
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Dg (2,2) = & ges (0, 0, oO oO (148) 


DY) eb with (OO, AO, ORG (Oe, Oo i (8, 4 OL) (149) 
ND OE, Ry nl gunk (Ome patel tel Pte, pcos eC), tell th poh ocr (8, de) (USO) 
Left manipulator point velocities are found by rearranging Equation (144). 
0 lo 
M1 
D, = 10, (tSt) 
0, : aN 


where Eqs (110) and (tlt) provide the expressions for Q. and Q, respectively, 
Using the same approach to find the yount velocities of the night manipulator, Point 


Pis expressed as a funetion of aght manipulator vanables 


’ re ) “ws 7) ) vw ¢ ’ } 2 

A Gy Ges (9, FO cg ad Ai gee (Ord tO. tie (152) 
\ s TRALEE a1 ona & 
Dy i Ge TO oO) Bosh COO) A ett, PO 1 eee) (153) 


Differentiate these equations and rearrange the terms m the form of 








~ D004, uN (i544) 

Oar 

where 

PR Go Biy ( , O eee ain 1O ee Oe) (TSS) 
meh Hee sin (0, Bote de, thes) (186) 
eR et Lene (187) 
Dy (2.2) a G gees (0 eh 1 Oe OL) (158) 
be tod) ee. 48 OO. Wed — Sy, Be, PO egret) - & SEU, Heap (tS9) 
bg C1) at ee (Oe Oe ed roe O. 1 +6. tery 4 OL) (160) 


Right manipulatoc out velocities are found by rearmanging Equation (134) 





‘l= Pa (161) 





where expressions for P, and P, are found by differentiating Eqs. (132) and (133). 
P, = X,-Gp(,-&,,) sind, (162) 


a = Yp+6p(G,-¢,,) cos@, (163) 
Manipulator joint accelerations are found by differentiating the expressions for 


velocity (Eqs. (144) and (154)). 


: oo co ithe) 0 
Qs} D,G9+D,6,+D2| *"} +d.) ° (164) 
Q, O1 Or > 


ee ae lig On 
“| = D399 +D,05+D, se BE 
P, Ogo) Op> 


Solving for joint acceleration gives 


Y -1] }Q, we TAO 

* =D, | ~ “ae, (166) 
O12 Q, oo 

3) . ® —_ ; —(l6 

= = v| E HO) Tread OJ tad Bon R} | (167) 
OR? P the 


where the accelerations of Points Q and P come from differentiating Eqs. (140)-(141) and 


(165) 














Eqs. (162)-(163). Derivatives of the D matrices are constructed by differentiating Eqs. 
(145)-(150) and Eqs. (155)-(160). 
2. Selecting Reference Trajectories 
Any path which connects the associated endpoints can be a reference trajectory. 
To help ensure that the spacecraft and payload do not experience any unnecessary jerk or 


excitation of flexible structures, one might further constrain the path such that the veloci- 
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ties and accelerations are zero at the endpoints. Because a reaction wheel is required to 

maintain spacecraft attitude, the reaction wheel torque history 1s a prime candidate for 

optimization. Possible performance indices include the integral of the absolute value of 

reaction wheel torque 

u 

ae [heel (168) 
t 


oO 


or the maximum reaction wheel torque. 


= abs (1U. neell) (169) 
A rigorous method for reference trajectory selection is to develop an optimal con- 
trol solution to the two point boundary value problem. The performance index in the opti- 


mal control problem is given by 


Teepe mina et (170) 
Using Eq. (168) as an example, 
L = |u,,| = [Dui (171) 
where 
(u = Lu, OLS Ube GENT CRSEERE dey} ) (172) 
and 
Die [1 00000 0) (173) 


The state equations must be formulated as first order differential equations as 


X= fx, uc, tl (174) 


Because the system dynamics of my problem are second order differential equa- 
tions, the state vector for the trajectory optimization 1s a combination of generalized dis- 


placements and velocities. 
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x= Jot gt (175) 


The resulting state equations are 


Vall: Gee) 
pe : _ | 8x8 8x8 x+ 
q O88 Oe KR 


where G and B are the same matrices as already found in Eqs. (94) and (95) respectively. 


8x7 


Q 
ut 8x] 
M'B)  [-M'G 


(176) 











Desirable boundary conditions are such that the payload is at rest with zero accel- 
eration at the beginning and end of the repositioning maneuver. However because the 
state vector does not contain accelerations, they cannot be specified as a boundary condi- 
tions. If the state vector is increased to include accelerations, then the first order state 
equations involve third order derivatives of the equations of motion rather than second 
order equations. This prevents including payload accelerations as part of the boundary 
conditions. To permit further development of the optimal control problem, the boundary 


conditions will be limited to desired positions and zero velocity. 


df 
xi) = tat) OF a (177) 


lacy o.) (178) 


x (t)) 


The Hamiltonian formed by combining the performance index with the state equa- 

tions 1s 
Alx(y,ucy,20,t) = L[xqy,ucn, +4 Oly cy, c,d (179) 
The performance index and the state equations are both linear with respect to the 
control vector, u. The consequences of this are that one cannot find a minimum by taking 
the gradient with respect to u and setting it equal to zero. The applicable control form is 
bang-bang. Separating the Hamiltonian into those terms which premultiply u and those 


which do not leads to the control law 
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ue =u sign(II,) (180) 
where 
H=H,+H,u (181) 
The other equations which must be satisfied are 


: H OL. OF 
es oO 7 ,T a 


= 182 
Ox Ox Ox ( ) 
Because the performance index 1s only a function of u, the first portion of the 

above necessary condition Is trivial. 


en (183) 


Pee ie 
Of . ; 
5. IS not as easily found. The M, G, A, and A matrices are all functions of the state 
xX 


vector. In addition, the complexity 1s increased by several matrix inversions in the expres- 


sion of fin the G and B matrices. Although an analytical expression may be theoretically 
possible, finding it was found to be extraordinarily tedious. 

Recall, however, that the usefulness of the reference trajectories 1s to specify the 
generalized coordinates, velocities, and accelerations. Therefore, a convenient form for 
the reference trajectory is as a polynomial function of time. The following development 
uses the payload attitude generalized coordinate to illustrate how the polynomial reference 


trajectories are applied. Let 

AO, = 9) (t,) — 9, (to) (184) 
where ty 1s the maneuver start time and ty 1s the final time. The duration of the maneuver 
is the difference between ty and ty. Op(to) and Op(t,) are the initial payload attitude and the 
desired final attitude respectively. If the desired reference path for the payload attitude in 


moving from initial to final conditions 1s a curve which can be represented as a polyno- 


mial function, f(t), where t is simply normalized time 
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(t~t,) 





one aie (185) 
(= 
then 
0» (1) =O, (1) #£ 04) (AD)) (186) 
| 
Op (= f(t) (A0,) ie) (187) 
ip 0 
Op (t= f" (t) ian) [ | (188) 
ref (ty = i) 2 


In order for Eq. (186) to produce the correct initial and final values for 0, , the 


Poe 
polynomial must be such that 

f(t=0) = 0 (189) 

f(t=1) = | (190) 

To produce zero velocity and acceleration at the initial and final conditions 


requires that f(t) also satisfy 


f (t=0) =0 (191) 
f'(t=1) =0 (192) 
f’(1=0) = 0 (193) 
f'(t=1) =0 (194) 


The minimum order polynomial which satisfies the boundary conditions of Eqs. 


(189)-(194) is 


f(t) = 60-151) 4 10t° (195) 
The expressions for payload reference trajectory using the fifth order polynomial 


become 


@, (t) = O,(t,) + (6t”- 151" + 107°) (A8,) (196) 
ref 
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Op (t) = (301 — 601” +30”) (AO,) cos (197) 





Op (t) = (1201 — 1801” + 607) (AO,,) i | (198) 
ref Cs 


The polynomial reference trajectory is also be applied to the other generalized 
coordinates which form the minimum set to describe the system (i.e. centerbody attitude 
and payload center of mass coordinates). The redundant generalized coordinates are cal- 
culated from the reference coordinates as described earlier. 

Higher order polynomials can increase the complexity of the path but offer the 
advantage that an infinity of polynomial coefficients satisfy the position, velocity, and 
acceleration boundary conditions. The selection of the coefficients affords an opportunity 
to optimize the reaction wheel torque. In this system, manipulator actuator torques are 
internal while the reaction wheel torque is the only external torque. Therefore, the reac- 
tion wheel torque will be equal to the rate of change of angular momentum which can be 


calculated directly from a reference trajectory. This technique is more computationally 


efficient because it does not require the construction of the G and B matrices. 
In general, the angular momentum about the inertial origin for each member of the 
system 1s 


H = I © +m (tr x v) (199) 
name ==] 


where 1 1s the moment of inertia of the it! body about its center of mass 
is the angular rate of the i" body 
m, 1s the mass of the ik body 
r 1s the inertial position of the i” body center of mass 


vis the inertial velocity of the ite body center of mass 
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The angular rate, position and velocity vectors were previously developed in con- 
nection with determining kinetic energy. Those expressions require some coordinate 
transformations to express all the terms with respect to the inertial coordinate frame. The 


change in angular momentum 1s found by differentiating Eq. (199) to produce 
H = Lo +m (r xa) (200) 
The total system change in angular momentum ts the sum of change in angular 
momentum for each of the members. After collecting terms with common angular veloc- 


ity or acceleration terms, the expression for the system change in angular momentum 1s 


given by 
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i = 1,Op +m, (XpYp- XpY,) (201) 
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Any polynomial reference trajectory that satisfies the initial condition concerning 


displacement cannot have a constant term. Polynomials which satisfy the velocity and 


acceleration initial conditions must not contain linear or quadratic terms. The general nth 


order polynomial reference trajectory has the form 
f(t) =a t+ a, jot! ta, tt? +... Fast? + ayt® +.a3t° (202) 
Derivatives are 


f(t) = na,t™! + (n-1)a,_ 1? + (n-2)a,.2t"3 + ... + Saget? + 4ayt? + 3a377 (203) 
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f(z) =n(n-1)a,t"? + (n-1)(n-2)a,.) 2"? + (n-2)(n-3)ay gts +... + 2ast? + 1Zagt4+G6a3t (204) 
When t=! and the final conditions, f(1) = 1, f (1) = 0 nad f (1) = 0, are substituted 


into Eqs. (202)-(204), these equations can be put into matnx form 


] | ] ] vee SG ee 
Oe= n (n-1) (n-—2) . 5 4 3I] ... | = [Wha (205) 
0 n(n-1) (n-1) (n-2) (n-2) (n-3) ... 20 12 6]! a 


The column vector of polynomial coefficients can be partitioned. One segment, 


as43, contains the coefficients for the third, fourth, and fifth order terms in Eq. (202). The 
other segment, aj,joh, Contains all of the coefficients of order six and higher. 
a = in (206) 
543. 


The W matrix can be partitioned accordingly. 


W = Wek Weal (207) 

One can then solve for the lower order polynomial coefficients in terms of the 
higher order coefficients by substituting Eqs. (206) and (207) into Eq. (205). The result 
specifies polynomial reference trajectory coefficients which satisfy the boundary condi- 


tions. 


1 


= ; 
45.43 ss W543 " i. MN enemen 
0 


(208) 


An optimal solution for a polynomial reference trajectory is found by using the 
MATLAB function fminu. This tool numenically finds the solution to an unconstrained 


function minimization problem using a quasi-Newton method. The function to be mini- 
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mized is the rate of change of angular momentum, Eq. (201), which can be found once a 
reference trajectory is specified. The user makes an initial guess for the higher order refer- 
ence trajectory coefficients. The lower order coefficients are calculated by Eq. (208). The 
MATLAB function then varies the higher order coefficients and recalculates the lower 
order coefficients as necessary to minimize change in angular momentum. One limitation 
to this technique ts that the algorithm may converge to a local rather than the global mini- 


mum. 
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Il. VALIDATION AND SIMULATION RESULTS 


The computer simulations presented in this chapter were obtained using the MATLAB 
subroutines included in Appendix B. The integrator uses 4"" and 5"" order Runge-Kutta 


formulas. See Appendix B for documented listings of the computer code. 


A. VALIDATION 

To verify the equations and find the programming bugs, test cases were developed. 
The simulations are analyzed to ensure that universal principles such as conservation of 
energy and angular momentum are not violated. Numeric values for the generic dual two- 
link manipulator system are contained in Table 1. The generic model is the strawman con- 
figuration that all of the test cases are based on with the exception of a few minor varia- 
tions. The variations will be pointed out in the appropriate test cases. The values for the 
generic model’s system properties are picked for uniformity and simplicity. The manipu- 
lator links and the payload are modelled as slender rods of uniform density. 

1. Conservation of Kinetic Energy 

In the first test case, no torques are applied and the initial velocities are nonzero. 

Under these conditions, the system links drift subject to the constraints of being pinned 
together. Since potential energy is zero and there are no external energy sources, kinetic 
energy should remain constant. The system begins with the payload parallel to a line 


drawn between the two shoulders and 0.75m away from them. The initial angular rate for 


the centerbody is chosen to be 6, = 2 deg/sec. The initial angular rate for the payload is 6, 
= -5 deg/sec. Initial velocities for the payload center of mass are -0.1 m/sec along the x 


axis and -0.05 m/sec along the y axis. The remaining generalized velocities are calculated 
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TABLE 1. GENERIC MODEL SYSTEM PROPERTIES 
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based on the values specified for the centerbody and payload. Initial angular rates for the 
manipulator links are 6,, = 6.6607 deg/sec, 6,, = -7.0457 deg/sec, 6,, = -2.7553 deg/sec, 


and 0p, = 14.9127 deg/sec. The graphical results from this test case are included in Fig- 


ures 4-8. As indicated in Figure 7, kinetic energy is conserved in this case. 
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Figure 4: Test Case I Angles 
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Figure 5: Test Case 1 Angular Rates 
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Figure 6: Test Case 1 Time Lapse Stick Figure 
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Figure 7: Test Case I Kinetic Energy 
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Figure 8: Test Case | Angular Momentum 


Test Case 2 is an extension of Test Case 1. This is still a case with nonzero initial 
velocities and no external torques. However, the system geometry is altered to be sym- 
metrical. In addition to conservation of kinetic energy, this test case will ensure that the 
symmetry is preserved. The physical alterations in the system involve moving the loca- 
tion of the left shoulder from 90 degrees to 135 degrees and decreasing the distance from 
the origin to the nght shoulder to 0.75 meters. The payload still begins centered between 
the shoulders and parallel to the y axis but is 1.2 m from the origin. To maintain symme- 


try, the initial velocities must also be symmetrical. The initial angular rate for the center- 
body is chosen to be 6, = 0 deg/sec. The initial angular rate for the payload is also zero. 
Initial velocities for the payload center of mass are zero along the x axis and -0.05 m/sec 


along the y axis. The remaining generalized velocities are again calculated based on the 


values specified for the centerbody and payload. Initial angular rates for the manipulator 


links are 6,, = 2.3188 deg/sec, 6,, = -7.6851 deg/sec, 62, = -2.3188 deg/sec, and 6,, = 


7.6851 deg/sec. This combination of system geometry and initial velocities 1s designed to 
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cause the payload to drift toward the origin without changing its attitude. Figures 9-13 
show the results from this test case. Kinetic energy is conserved and symmetry 1s pre- 


served. 
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Figure 9: Test Case 2 Angles 
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Figure 10: Test Case 2 Angular Rates 
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Figure I1: Test Case 2 Time Lapse Stick Figure 
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Figure 12: Test Case 2 Kinetic Energy 
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Figure 13: Test Case 2 Angular Momentum 


2. Conservation of Angular Momentum 

As long as a system does not include external torques, one expects that angular 
momentum should be conserved. The joint actuators provide internal torques while the 
reaction wheel is the onlv external source. Test Cases | and 2 did not include a reaction 
wheel and are therefore subject to investigation with respect to conservation of angular 
momentum. Both cases do satisfy the requirement as indicated by Figures 8 and 13. Fur- 
thermore, due to the symmetry in the system in Test Case 2, the angular momentum of the 
left manipulator links should be cancelled out by the angular momentum of the night 
manipulator links. The centerbody and payload should not have any angular momentum. 
Consequently, angular momentum for the system should not only be conserved, it should 


be zero. Figure 13 show that the angular momentum remained virtually zero. The non- 


zero values of about 3x10°!” are well within the integration algorithm tolerance of or’. 
Test Case 3 returns to the generic system from Table 1. Initially, the system is at 


rest. Constant torques are applied at both shoulders and nowhere else. The torques are 


2 


0.01 N-m applied tn the positive direction at the right shoulder and the negative direction 
at the left shoulder. Because the joint torques are internal to the system, angular momen- 
tum must still be conserved even though kinetic energy won’t be. Furthermore, since the 
system began at rest, the angular momentum should remain at zero. The results are shown 
in Figures 14-18. Although the angular momentum did not remain identically equal to 


-6 


zero, their magnitudes of less than 2x10°/ are within the 10° tolerance placed on the inte- 


gration algorithm. 
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Figure 14: Test Case 3 Angles 
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Figure 15: Test Case 3 Angular Rates 
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Figure 16: Test Case 3 Time Lapse Stick Figure 
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Figure 17: Test Case 3 Kinetic Energy 
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Figure 18: Test Case 3 Angular Momentum 


Test Case 4 is similar to Test Case 3 but the symmetrical system geometry is used 
instead of the generic geometry. This change should produce symmetric motion and zero 


angular momentum. The reaction wheel is still disabled. Figures 19-23 indicate the sys- 


tem reacted as expected. Changing the torques to time varying profiles rather than con- 


stants led to similar results. 
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Figure 20: Test Case 4 Angular Rates 
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Figure 21: Test Case 4 Time Lapse Stick Figure 
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Figure 22: Test Case 4 Kinetic Energy 
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Figure 23: Test Case 4 Angular Momentum 


3. Wheel Torque and Constraints 

The remaining test cases involved using the reaction wheel on the centerbody. The 
wheel’s function was to maintain attitude pointing. The system begins at rest. The torque 
applied by the wheel is an external torque in this model. Therefore, its value must be the 
same as the change in angular momentum. The wheel torque is found by means of the 
inverse kinematics equations in Chapter II. These calculations are entirely independent of 
finding the change in angular momentum. After a simulation is finished, a separate sec- 
tion in the program code calculates the change in angular momentum using the general- 
ized coordinates, velocities and accelerations produced by the integration. These values 
are plotted along with those of the reaction wheel torque. A sample plot is contained in 
Figure 24. This particular plot is for the case of a fifth order polynomial reference trajec- 
tory. The rest of the plots associated with this case are presented later in the Simulations 
section. The validation tests concerning conservation of kinetic energy and angular 


momentum required special circumstances to create those conditions. The requirement 
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that the reaction wheel torque equal the change in angular momentum 1s more universal. 


It is a venfication check performed with every simulation involving a reaction wheel. 
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Figure 24: Sample of Wheel Torque and Change in Angular Momentum vs. Time 


An even more universal check also performed with every simulation is the require- 
ment that the constraint equations (Aq+A, = 0) are satisfied. Figure 25 shows a sample 
plot. This plot was also taken from the fifth order polynomial reference trajectory case. 
The values plotted represent the four constraint equations contained in Eqn 72. The non- 
zero values are attributed to numerical errors created by the integration. 

Finally, a common sense check also performed with every simulation 1s simply to 
verify that the payload was repositioned to the desired final location. This cannot happen 
if the torques applied to the system were incorrect. This test is a necessary but not suffi- 


cient condition that the code operates correctly. 
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Figure 25: Sample of Constraints vs. Time 


B. SIMULATIONS 

This section presents results from several simulations of an analytical model. The 
desired payload repositioning maneuver is illustrated in Figure 26. The final position for 
the payload involves a 90 degree rotation and the nght endpoint finishes where the left 
endpoint started. 

1. Lyapunov Point Controller 

In the first simulation, the repositioning is done entirely by the Lyapunov control- 

ler without the benefit of a reference trajectory. The behavior is that of a point controller 
with an initial displacement rather than that of a tracking controller. Due to the absence of 
the weighted norm reference torques, this controller cannot be consider to have coopera- 
tive nature. Figure 27 presents the angular displacement history. The asterisks on the 
right side of the plot indicate the desired final angles. Although the system is approaching 
the desired final geometry, it has not completely settled down even after 40 seconds. Posi- 
tion errors (Figure 28) are still present as well as nonzero velocities (Figure 29). Also note 


that the reaction wheel torque 1s quite high during the maneuver (Figure 30). The joint 
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actuator torques are considerably less than the reaction wheel torque. They are not identi- 


fied individually because the most important feature of Figure 30 is the reaction wheel 
torque. As a quantitative measure of this controller’s quality, {\u,,|/dt produces a value of 


17.3841. The oscillatory nature of the system is evident in the angular position and veloc- 
ity plots. Despite the oscillations, however, the stability of the controller is also iflus- 
trated. Figure 31 depicts the system geometry at several instances during the maneuver. 
The left manipulator links actually cross over each other. In experimental hardware, the 
links would collide instead. Figure 32 removes the clutter that 1s present in Figure 31 and 
displays only the initial and final geometry. The Lyapunov point controller also does a 
poor job of maintaining the centerbody attitude. This is clearly evident in Figures 27 and 


31. The attitude error peaks at about 16 degrees. 
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Figure 26: Desired Repositioning Maneuver 
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Figure 27: Lyapunov Point Controller Angles 
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Figure 28: Lyapunov Point Controller Displacement Errors 
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Figure 29: Lyapunov Point Controller Angular Rates 
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Figure 30: Lyapunov Point Controller Command Torques 
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Figure 32: Lyapunov Point Controller Initial and Final Stick Figures 
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2. Lyapunov Tracking Controller 


This controller uses the following equation to calculate control torques 


Sel C-K8g4+C) ue (Cyd-Cy a.) — (C3- C3) -KpSq} 209) 


This equation was developed in the analytical chapter and repeated here for conve- 
nience. The command torques are based on errors with a reference trajectory. Reference 
torques which resulted from minimizing a weighted norm of the actuator torques associ- 
ated with the reference trajectory are also included. 

a. 5!" Order Reference Trajectory 

In this simulation, a fifth order polynomial reference trajectory is applied to the 
payload generalized coordinates. The payload coordinates displacements, velocities, and 
accelerations resulting from this polynomial are depicted in Figure 33. When calculating 
the reference torques from the inverse kinematics, the six joint actuators are all weighted 
equally. The maneuver time is selected to last 10 seconds. Asis demonstrated in Figures 
34-36, the system successfully moves from initial conditions to desired final conditions. 
The displacement errors are less than On deg (Figure 34). The command torques (Figure 
37) are an order of magnitude smaller than for the previous simulation which lacked a 
reference trajectory. Evaluating fju,,|dt leads to the dramatically improved value of 
0.5746. More importantly, the centerbody attitude 1s maintained throughout the maneuver. 


Figure 38 shows the time lapse depiction of the maneuver. 


65 


Dispiacemem vs Normalized Time 





06 
Displacement 
04 


Ses 3 
f(t) = 6t-15t+10t 


0.2 


0.5 
t (sec) 


Velocity vs Normalized Time Acceleration vs Normalized Tune 
5 wot 





Velocity 10 


0.5 





0 : _ oe 
0 05s 1.0 0 0 § 1.0 
t (sec) t sec} 


Figure 33: 5'" Order Reference Trajectories 
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Figure 34: 5'> Order Angles 
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Figure 35: 5" Order Displacement Errors 
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Figure 36: 5‘) Order Angular Rates 
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Figure 37: 5¢h Order Command Torques 
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Figure 38: 5‘ Order Time Lapse Stick Figure 
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b. 8"" Order Reference Trajectory 

By increasing the order of the reference trajectory polynomial while 
maintaining the same boundary conditions concerning velocity and acceleration, one 
hopes to achieve improved performance. For example, the domain of all sixth order 
polynomial functions includes all fifth order polynomial functions as a subset. Therefore, 
when searching all sixth order polynomials for coefficients which will minimize the cost 
function, one possible solution is the fifth order polynomial already used. Using the 
function minimization routine discussed in the previous chapter, a sixth order polynomial 
function was found. Although there was some improvement, the change in performance 
was not significant. The same was true for a seventh order function. An eighth order 
function is presented here. It was hoped that the increased order would be enough of a 
departure from the fifth order cause to produce significant improvement in reducing the 
centerbody disturbance torque. The algorithm converged to a solution for the eighth order 
polynomial after running approximately two hours on a personal computer with an Intel 
486-DX50 cpu. The resulting trajectories are very similar to those for the fifth order case 
and are displayed in Figure 39. The most obvious difference is a lack of symmetry. Plots 


for this case are contained in Figures 40-43. The value of ||u,,|dt for this case was 0.5705. 
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Figure 40: 8'" Order Angles 
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Figure 41: 8'" Order Angular Rates 
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Figure 42: 8! Order Command Torques 
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Figure 43: 8'" Order Time Lapse Stick Figure 


3. Modified Lyapunov Tracking Controller 

This simulation represents a compromise between the Lyapunov point controller 
and the Lyapunov tracking controller. Because the Lyapunov point controller does not use 
a reference trajectory, the cost function which minimizes the weighted norm of the actua- 
tor torques is completely bypassed. The modified Lyapunov controller removes the refer- 
ence torque term from the command torque calculation (Eqn 209) but calculates command 
torques based on errors with a reference trajectory. Like the Lyapunov point controller, 
the modified Lyapunov tracking controller does not minimize a weighted norm of the 
actuator torques and is therefore not a cooperative controller. The angle histories in Figure 
44 exhibit less of the oscillatory nature than the point controller simulation, but the accu- 
racy shown in Figure 45 is considerably worse than the reference trajectory simulations. 
Figures 46-48 also illustrate behavior better than the point controller but not as good as 
when command torques are found using Eqn 209. The magnitude of the command torques 
show an order of magnitude improvement over the point controller. This is directly attrib- 


utable to using intermediate reference points on the way to a desired final state rather than 


Ue 


attempting to achieve the desired final state all at once. Calculating flo ale produced a 


value of 2.4523. The time lapse figure shows that the motion 1s much less wild but the 


centerbody attitude error is still noticeable. 
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Figure 44: Modified Lyapunov Tracking Controller Angles 
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Figure 45: Modified Lyapunov Tracking Controller Displacement Errors 
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Figure 46: Modified Lyapunov Tracking Controller Angular Rates 
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Figure 47: Modified Lyapunov Tracking Controller Command Torques 
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Figure 48: Modified Lyapunov Tracking Controller Time Lapse Stick Figure 


4. Comparison of Controllers 

Table 2 summarizes the results of the Lyapunov point controller, the two Lyapunov 
tracking controller cases, and the modified Lyapunov tracking controller. The point con- 
troller clearly has the worst performance with high reaction wheel torque and large center- 
body attitude error. The tracking controller performs much better. Reaction wheel torque 
is greatly reduced and centerbody attitude error is eliminated. As expected, increasing the 
order of the polynomial reduces the reaction wheel torque further, but the improvement ts 
relatively small. The modified tracking controller strikes a compromise between the point 


controller and the tracking controller. 
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TABLE 2. COMPARISON OF HYPOTHETICAL MODEL 


SIMULATIONS 
Centerbody 
|U nas Attitude Cooperative 
Error (deg) 


Point Controller 17.3841 2.9365 16.2261 
Tracking 0.5746 | 0.0961 0.0000 | Yes | 
Controller 0.5705 | 0.0885 0.0000 


Modified Tracking 24523 | 0.3950 
Controller 
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IV. EXPERIMENTAL WORK 


The experimental phase of this research was conducted on the Spacecraft Robotics 
Simulator (SRS). The SRS ts a derivative of the Flexible Spacecraft Simulator (FSS) ini- 
tially developed by Watkins [Ref 17] and later modified by Hailey [Ref 18]. Sorensen 


[Ref 18] began the work to convert the FSS into the SRS. 


A. SETUP 

The SRS permits experimental investigation of two dimensional robotics motion and 
rotational spacecraft dynamics. The SRS ts illustrated in Figures 49 and 50. The simula- 
tor hardware 1s floated on an eight foot by six foot granite table by means of a thin layer of 
air supplied by an external source. The table is polished to within 0.001 inch peak to val- 
ley and leveled to prevent gravitational accelerations from impacting the motion across its 
surface. The following sections descnbe the simulated spacecraft with its associated sen- 
sors and actuators and the controller which together form the SRS. The spacecraft compo- 


nents are the centerbody, two manipulators, and a payload. 
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Figure 50: System Top View 


1. Centerbody 
The centerbody is a 30 inch diameter, 7/8 inch thick aluminum disk. The center- 
body carries a position sensor, rate sensor, momentum wheel, thrusters, and an air tank to 
power the thrusters. The centerbody also serves as the mounting platform for the manipu- 
lators. The centerbody is floated by a central air bearing and three air pads located at 120 
degree intervals near the outer edge. The air pads are each capable of floating 60 pounds 
when the air pressure supplied to the pads is 80 psi. The air bearing is attached to an over- 


head I-beam which restricts to motion of the centerbody to rotation only. 


79 


Centerbody angular position is sensed by a Rotary Vanable Displacement Trans- 
ducer (RVDT) mounted directly above the air bearing. The RVDT is a model R30D man- 
ufactured by Schaevitz Sensing Systems. Its linear range is restricted to + 40 degrees. 
Centerbody angular rate is measured by a rate transducer manufactured by Humphrey, Inc. 
The instrument has a range of +100 deg/sec and a minimum threshold of 0.01 deg/sec. 

Centerbody angular position 1s controlled by a momentum wheel. The momentum 
wheel speed is measured by a tachometer contained in the servo motor which drives the 
momentum wheel. The centerbody momentum wheel is powered by a model JR16M4CH/ 
F9OT servo motor manufactured by PMI Industries. Characteristics of this motor are sum- 
marized in Table 3. Although the centerbody also carries two thrusters, they are not used 


in this research. 


TABLE 3. MOMENTUM WHEEL MOTOR 
CHARACTERISTICS 


Rated Output Speed (rpm) 


2. Manipulators 










Two two-link manipulators are mounted GO degrees apart on the centerbody. Each 
manipulator has three joints. The shoulder joints are supported by the centerbody while 


the elbow and wrist joints are supported by two air pads apiece. The links between the 
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joints are stiff laterally but permit some flexibility vertically. This feature increases the 
tolerances on the air pad height adjustment. 

Left arm joint angles are measured by the same model RVDT as is used on the cen- 
terbody. All three of the left arm actuators are series 9FGHD servo disk motors manufac- 
tured by PMI Industnes. Joint angles on the right arm are sensed by encoders purchased 
with the joint actuators. The encoder resolution is 0.005 degrees. The right arm joint 
actuators arm are harmonic drive dc servo actuators manufactured by HD Systems, Inc. 
The shoulder actuator is model RFS-25-6018-E036AL while the elbow and wrist actua- 
tors are model RFS-20-6012-E036AL. Specifications for the three types of joint actuators 


are contained in Table 4. 
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Figure 51: Left Manipulator Top and Side Views 
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Actuator/Encoder 





Figure 52: Right Manipulator Top and Side Views 


TABLE 4. MANIPULATOR ACTUATOR CHARACTERISTICS 


RFS-25-6012 | RFS-25-6018 9FGHD 


Rated Output Speed (rpm) 
Rated Voltage (volts) 
430) 












Rated Current (amps) 

Rated Torque (in-lb) eee 
Height(in) | 88 
Weieht (by [93 
Footprint ny [a3 


! Side of square 
Diameter of eircle 
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The joint actuators are all driven by Kepco power supplies. These bipolar, pro- 
grammable, linear amplifiers can be controlled manually from the front panel or con- 
trolled remotely with a +10 volt signal. In this application, the power supplies are 
operated in the current control mode with the voltage and current limits manually set con- 
sistent with the values in Table 4. The specific power supply models and their characteris- 


tics are summanized tn Table 5. 


TABLE 5. POWER SUPPLIES CHARACTERISTICS 


Model BOP 72-6M BOP 72-3M_ | BOP 20-10M 


; Right Elbow, | All Left Arm 
Actuators Controlled Right Shoulder Right Wrist ae 


+72 volts +72 volts +20 volts 
+6 amps +3 amps +10 amps 


Closed Loop Gain 0.6 (amp/volt) | 0.3 (amp/volt) | 1.0 (amp/volt) 


3. Payload 


DC Output Range 





The payload is a rigid bar mechanically fastened to the ends of both manipulators. 
The payload is supported entirely by the air pads on the manipulator wnst joints. Ballast 
can be added to the payload to change the mass and inertia charactenstics of the system. 
This allows for the construction of cases in which the mass of the payload is nontnvial 
compared to the spacecraft centerbody. The payload contains no sensors or actuators. 
Payload position is derived from the manipulator joint angles. 

4. Controller 

The AC-100 programmable controller manufactured by Integrated Systems, Inc. 

controls the SRS. The AC-100 includes an Intel 80386 Application Processor, an Intel 


80386 Multibus II Input/Output Processor, an Intel 80386 Communication Processor, and 
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Intel 80387 Coprocessor, a Weitek 3167 Coprocessor, and Analog-To-Digital and Digital- 
To-Analog Data Translation DT2402 Input/Output Board, two INX-04 Encoder and Digi- 
tal-To-Analog Servo Boards, and an Ethernet Interface Module. The AC-100 also 
includes software installed on a VAX 3100 Series Model 30 workstation. The software 
permits design of a controller in block diagram graphical form and conversion of the dia- 
gram to C language programming code. The user is also able to design an interactive ani- 
mation window to operate the controller. The AC-100 receives input signals from the 
sensors and the graphical user interface. AC-100 output signals go to the power supplies 
driving the actuators or to the graphical user interface for display. 
5. System Integration 

The differences between the ideal world of an analytical simulation and the real 
world of actual hardware became apparent during system integration. A few problems 
arose then requiring some modification of the experiment. The first problem concerned 
floating the centerbody. It exhibited a noticeable resistance to rotation. This is due in part 
to the air pressure of the available air suppiy. Because it was only 40 psi, the air pads per- 
formance was degraded by a factor of two. Pnor to mounting the manipulators, the cen- 
terbody weighed approximately 125 lbs. Adding the shoulder motors increased the 
centerbody weight to 145 lbs. The extra weight may have been enough to overwhelmed 
the centerbody air pads. A second contributing factor to the centerbody drag 1s the inabil- 
ity of the central air bearing to function except under very low lateral loading. The modi- 
fication to the experiment created by the centerbody problem 1s to not float the centerbody. 

A second problem involved using the RVDTs. As envisioned, the experiment 
requires one RVDT for the centerbody and three for the left manipulator joints. The Space 
Dynamics laboratory has a total of three in stock. Although a fourth has been ordered, it 
did not arrive in time to be used. Using the existing RVDTs revealed another problem. 


Data acquisition of the RVDT signal by the AC-100 exhibited a random toggling of the 
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sensed value between a good reading and a value of zero. Because the angle information 
is critical to calculating actuator commands, this behavior is unacceptable. Consultation 
with the Integrated Systems technical support group revealed that this type of behavior 1s a 
bug within the AC-100 software which has been corrected in more recent versions. Use of 
the newer version was not possible because it requires upgrading the VAX workstation 
hardware and an updated version of the VMS operating system. The experimental modifi- 
cation used to overcome these difficulties is to derive the joint angles and velocities of the 
left manipulator by using the sensed information from the right manipulator encoders. 
Velocities were not sensed directly but approximated by the change in displacement which 
occurred since the last sample divided by the sample rate 

A third obstacle involved the limitations of software to design the control algo- 
rithm. The block diagram construction method did not permit convenient matnx opera- 
tions. Matrix multiplication must be programmed in an element by element basis. Matnx 
inversion must also be calculated by constructing a series of blocks to find each element. 
This handicap 1s not serious when the system equations of motion are of low order. How- 
ever, the dual two link manipulator configuration 1s eighth order and beyond the practical 
means of programming complex matrix operations, especially matnx inverses. Recall that 
the command torques are calculated by the following relationship 

ic, C.) Po (-K,6q+C, wu -(Cyd@-Cy 4.) - (C3-C3_) — Kp 8a) 
(210) 

When the differences between the actual path and the reference path are small, this control 
law simplifies to something very similar to a PD controller. Therefore, the control law 
used by the experiment is a PD controller rather than the complete Lyapunov controller. 

Performance differences between the left and nght manipulator actuators also pre- 
sented some problems. Because of the actuator redundancy, any three joint actuators 


should be enough to follow a reference trajectory. This fact can be demonstrated by using 
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only the three nght joint actuators. However, the same trajectory is not possible with only 
the left actuators. The torque provided by the left joint actuators is insufficient to com- 
pletely overcome the internal friction of the right joint actuators. Even when the left joint 
actuators are commanded manually from the front panel, there is no correction to reduce 
the position error. When steadily increasing the commanded current to the motor, the cur- 


rent limit is reached before the motor responds. 


B. RESULTS 

The reference trajectory for the experimental phase 1s slightly different from that used 
in the analytical section. The reference maneuver still involves a 90 degree rotation of the 
payload with the right endpoint ending where the left endpoint began. The differences 
arise from the system parameter such as lengths and masses not being the same as in the 
generic hypothetical model. The desired reference maneuver is depicted in Figure 53. 
Results are shown in Figures 54-58 and summarized in Table 6. The sudden changes from 
believable values to zero in the figures are problems with the data acquisition software and 


do not indicate actual changes in the experimental hardware geometry. 
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Figure 53: Desired Experimental Repositioning Maneuver 
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TABLE 6. EXPERIMENTAL ERROR ANGLES 


Errors (deg) 


a ; Maximum 
Magnitude 
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V. SUMMARY AND CONCLUSIONS 


A. SUMMARY 

The dynamics of a dual two-link manipulator system which ts repositioning an already 
grasped payload have been analyzed. The equations of motion for the system were devel- 
oped using Lagrange’s method. The resulting equations were highly nonlinear, coupled, 
second order differential equations. Given any reference trajectory, the actuator torques 
that will produce that trajectory were calculated to minimize a weighted norm of the 
torques. Stability of the system during the repositioning maneuver was ensured by a con- 
troller derived from Lyapunov stability theory. Equations for deriving joint angles from 
centerbody and payload reference values was also developed. Polynomial reference tra- 
jectories were presented as an attractive means to specify a reference trajectory. 

The analytical model was validated using test cases in which some results could be 
predicted in advance. The model demonstrated conservation of energy when no torques 
were applied. It also exhibited conservation of angular momentum whenever the reaction 
wheel was disabled. The model also maintained symmetric geometry in the appropriate 
test cases. In cases which used the reaction wheel, conservation of energy and angular 
momentum did not apply, However, comparison of the change in angular momentum with 
the reaction wheel torque provided validation. Finally, in all test cases as well as simula- 
tions, the constraints were satisfied as measured by Aq+A, = 0. 

Results from simulations indicated that the Lyapunov point controller, although stable, 
behaved poorly. Large centerbody attitude errors, high command torques, and wild oscil- 


lations make this controller undesirable for large repositioning maneuvers. The Lyapunov 
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tracking controller exhibited dramatic a improvement in performance. Centerbody atti- 
tude errors were removed and reaction wheel torque decreased significantly. 

The experimental phase revealed that the controller required further simplification for 
compatibility with the laboratory resources. Acceptable results were obtained using a PD 
control law with a reference trajectory. 

The objectives of this research were to 1) develop a stable control law that facilitates 
cooperation among the manipulators as they reposition the payload, 2) minimize the joint 
actuator effort, 3) reduce the disturbance torque transmitted to the spacecraft main body 
by the manipulator motion, and 4) validate the analytical development with experimental 
results. The Lyapunov controller satisfies the first objective. The second objective is 
achieved by the weighted norm calculation of the actuator torques. Reduction of the cen- 
terbody disturbance torque is accomplished through reference trajectory selection. 
Although a ngorous application of classical optimal control techniques proved impracti- 
cal, a polynomial reference trajectory in which the coefficients were selected to reduce the 
disturbance torque was easily applied. Difficulties were encountered with regards to the 
fourth objective, experimental work. The controller developed analytically could not be 
directly transferred to the laboratory. This was due to a combination of hardware limita- 
tions and real world conditions instead of the ideal environment of the analytical model. 
The controller was adapted to the realities of the laboratory and resulted in successful 


accomplishment of a payload repositioning maneuver. 


B. ORIGINAL CONTRIBUTIONS 
A simulation tool has been developed to analyze the dynamics of a space based robot- 


ics system. Some of the features of this tool include: 
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(1) rotational motion of the spacecraft centerbody and planar motion of the manip- 


ulators and payload; 


(11) minimization of a weighted norm of the actuator torques based on a user sup- 
plied weighting matrix; 

(ii) calculation of polynomial reference trajectory coefficients to produce a local 
minimum for the integral of the absolute value of the disturbance torque based 
on a user supplied order for the reference polynomial and an initial guess for 


the coefficients: 


(iv)  areference trajectory with zero velocity and acceleration at the beginning and 


end of the maneuver; 


(v) | aLyapunov controller which guarantees stability in the face of perturbations 
between the reference trajectory and the actual dynamics caused by errors in 
the initial conditions. 

An experimental test bed was also developed. This effort involved the design of the 

manipulator components and the development of a real time controller. This test bed 
remains in the Spacecraft Dynamics and Control Laboratory and is available for follow-on 


work. 


C. RECOMMENDATIONS FOR FURTHER STUDY 

As with any research, this work answers some questions but raises others. One of the 
areas that could receive further attention is the selection of the Lyapunov controller gains. 
The theory requires positive definite matrices to ensure stability but offers no insights con- 
cerning selection of the matrices to improve performance. For any given set of controller 


matrices, one expects the relative merits of the point controller, tracking controller, and 


ee) 


modified tracking controller to remain the same. However, still better performance might 
be achieved across the board if the gains were optimized. 

Rather than merely changing Lyapunov controller gains, one might investigate another 
Lyapunov controller by beginning with a different candidate Lyapunov function than the 
one presented here. The choices are infinite and the results and performance difficult to 
predict. 

Trajectory optimization 1s another area that would benefit from further work. The 
function minimization algorithm used to select polynomial coefficients converged to local 
minima solutions depending on the initial guess for the coefficients. The search for a glo- 
bal minimum for a particular order polynomial requires further investigation. An alternate 
approach with respect to trajectory optimization 1s to use some function other than a sim- 
ple polynomial to describe the trajectory. Possible trajectories might be Tchebycheff poly- 
nomials, Legendre polynomials, or Fourier series. 

To help bridge the gap between the analytical model and the real world hardware, one 
could consider modifying the controller to include joint friction, actuator backlash, sensor 
noise, and flexibility. One could also consider using a minimum generalized coordinate 
formulation. One might also attack the differences from the hardware perspective by 
seeking components that more closely resemble those in the analytical model. Another 
improvement in the experiment would be to replace the existing joint velocity approxima- 
tions with either an observer or an actually velocity measurement. 

Finally, it’s a three dimensional world. Extending the analytical model and, if possi- 


ble, the laboratory experiment to include out of plane motion should be considered. 
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APPENDIX A: EXPERIMENTAL CONTROL BLOCK 
DIAGRAMS 


This appendix includes the block diagrams of the System Build super blocks made to 
control the SRS. The heirarchy among the super blocks is illustrated in Figure 59. Both is 


the parent superblock. The ohters are lower level super blocks. 


Both _ 


Parameters 


Trajectories 


Encoders 


LeftAngles 


Part] 


Controller 





Figure 59: Super Blocks Hierarchy 


The block diagram for super block Both is shown in Figure 60. Inputs into the dia- 
gram include the sensor signals from the hardware and user operated dials to select the 
controller gains and enable switches which select the combination of joint actuators to 
enable. The outputs include commanded, reference, and error signals for each of the cen- 


terbody angle, joint angles, and payload angle. Motor current commands to the Kepco 
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power supplies are also outputs. Block 56 contains the system parameter values for the 
experimental hardware. This block is expanded and displayed in Figure 61. Block 8 con- 
tains the position and velocity values for a reference trajectory in a look-up table. It also 
contains a table to reset the system back to its original geometry to permit rerunning the 
reference trajectory. This block is expanded in Figure 62. Conversion of the encoder 
pulses from the right manipulator into angle and angle rate information is done in Block 7 
which is expanded in Figure 64. Conversion of the encoder pulses from the night manipu- 
lator into angle information for the left manipulator is done in Block 49. Details of this 
block are shown in Figures 64-64. The PD controllers which convert the error signals into 
actuator commands are in Block 40. This block is expanded in two parts. The actuator 


commands for the right and left manipulator are shown in Figures 68 and 69 respectively. 
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Overall Control Block Diagram 


Figure 60: 
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Figure 61: Parameters Block Diagram 
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Figure 62 
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Figure 64: 
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Figure 65 
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Figure 66: Part 2 Block Diagram 
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Figure 67 
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Figure 68: Right Manipulator Controller Block Diagram 
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Figure 69: Left Manipulator Controller Block Diagram 
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APPENDIX B: MATLAB CODE 


The following listings are the MATLAB code used for the analytical simulations. The 
modules are included in alphabetical order. The hierarchical relationship between the 


modules is illustrated in Figure 70. The integration modules ode2 and odemin are minor 


~* — A eon —————EE 


variants of the MATLAB supplied module ode45. The modifications permit more param- 
eters to be passed to and from these modules without having to include the extra variables 


in the state vector. Similarly, fminu2 modified the MATLAB unconstrained function min- 


MainOpt 
_ 
MainMin 
ale 
odemin 
- 
Main2 RefMin2 
- 7 


AngMo2 


imization module, fminu, to include some diagnostic statements while running. 


1 a 
Draw3 Eqn2 
in qn a 
AngMo Ref2 
| | 
Matx Matx Matx 


MatxFix MatxFix MatxFix 


AngMo2 AngMo2 





Figure 70: MATLAB Modules Hierarchy 
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. AngMo 


% Filename is "AngMo.m" 
% This file calculates the angular momentum of the een 
function [Hs] = AngMo(Ls,Ms,CMs,Is,Q,Qdot) 


% OUTPUT: 

% Hs = 1x7 row vector of angular velocities. The first element is for 
% the centerbody. The next four elements arc for the left upper 
% and lower arm followed by the nght upper and lower arm. The 
% last two elements are for the payload and a total of all the 


% previous elements. [HO HL} HL2 HRI HR2 HP 11 Total] 
% 


Vane er. 

% Ls = 7x1 veetor of lengths (m) 

% Ist element = distance from origin to left arm mount 

% 2nd & 3rd elements wri left arm (from shoulder toward wrist) 
% 4th element = payload length 

% Sth & 6th elements wrt nght arm (from wrist toward shoulder) 
% 7th element = distance from right arm mount to ongin 


% (LO, ED Poe ee RZ lO) 
% Ms = 6x1 column vector containing the masses (kg) 


% Ist element = mass of spacecraft centerbody 

% 2nd & 3rd elements = mass of left arm (upper arm then lower arm) 
% 4th & Sth elements = mass of right arm (upper arm then lower arm) 
% 6th element = payload mass 


% [MO; ML1; ML2; MR1; MR2; MP} 

% CMs = 6x1 column veetor containing center of mass locations 

% [LcO: Vell, Ech? Eckdn icR2 her) 

% 1s = 6x1 column vector containing the moments of inertias about the 


% respective body's center of mass (kg m“%2) 

% Ist element = inertia of spacecraft centcrbody 

% 2nd & 3rd elements = inertia of left arm (upper arm then lower arm) 
% 4th & 5th elements = inertia of mght arm (upper arm then lowcr arm) 


% 6th element = payload inertia 

% HO Tet a 2 TR IR2 TE] 

% Q= 8x1 column vector of generalized coordinates 
% Qdot = 8x1 vector of generalized velocities 


WMMWVANMAMMMMMMMMMMMMMMM%M MMMM 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
MUMMMMMVMMMMMMAMMANNMNMNMMMMMMMMMM%Y% 
% Lengths (m) 
LO = Ls(1); 
LS se). 
L2— s3): 
LP = Ls(4), 
R2 = Ls(5); 
= Ls(6); 
RO =Ls(7), 


“% Member masscs (kg) 
MO = Ms(1); 

ML] = Ms(2); 

ML2 = Ms(3); 

MR] = Ms(4); 

MR2 = Ms(5); 

MP = Ms(6); 


% Center of mass distances (m) 
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LcO =CMs(1); 

LeL] = CMs(2); 

Lel.2 = CMs(3),; 

LecR] = CMs(4), 

LeR2 = CMs(5), 

LcP = CMs(6); “measured from Icft cnd 


% MOI about ccntcr of mass 
IO = I[s(1); 
1.1 =Is(2); 
IL2 =1s(3); 
IR} = Is(4); 
IR2 = Is(5); 
IP =Is(6); 


% Coordinates (rad & m) 
Tho = Q(1): 

ThL1] = Q(2); 

Th). 2 = Q(3); 

ThR] = Q(4): 

ThR2 = Q(5); 

ThP = Q(6),; 

me (7), 

lee OCS): 


% Coordinate Rates (rad/sec & m/sec) 
ThOd = Qdot(1); 

Thi. 1d = Qdot(2); 

ThL2d = Qdot(3), 

ThR 1d = Qdot(4); 

ThR2d = Qdot(5); 

ThPd = Qdot(6); 

XPd = Qdot(7), 

YPd = Qdot(8); 


% Angular Momentum 

110 = ThOd*(10 + MO*Lc0%2),; 

eee nOd *(11 1+ML1*(LO*%2+L¢L 1*24+2*L0O*LeL 1] *cos(ThL 1))) + .. 
Tiina ie leeMilel *(Cele “2 +LO*LeL 1 *cos(lhL1))): 

Rleeees Od (IL2tML 2*(LO*24+1,142+L¢eL2%24+2*71.0*1, 1) *cos(ThL)) + ... 
Ea ble eck 2 *cos( i hizZ)2*LO*LeL2*cos( tht i+inL2))) +... 
eta CE 2ML2*(L 172 +Lel2*°24+L0* 1) *ces( iil)... 

Pele eel 2*cos(Thi2)+LO*LcL2*cos(Thi-l4 Thl.2))) + -.. 
ined 24+MiL2 "(Lich 2°24 *Lel2*cos(hL2) +... 
PO cl 2*cos( Thi J+ ThL2))): 

HR1 = Th0d*(IR1+MR1*(RO%2+LcR 142+2*RO*LCR 1 *cos(ThR1))) + ... 

ThRId*(IRI+MR1*(LcR14%2+R0*LcRI *¥cos(ThR 1))): 


fee = 1n0d*(IR2+MR2*(RO*%2+R1%2+LeR2%24+2*ROFR I *cos(ThR1) +... 


Pe ee Cost IhR2)+2*ROtLcR2*cos( Tink 1+ TnR2Z)))-4 .... 
Pi ld= Re eM 2*(RI7~27+lcR2°2+RO*RI*cosC hk) + ... 
Piel A liek 2 *cos(ThR2)+RO*LcR2*cos( FhRI+ThR2))) +... 
Nik 2d* (kK 27Vik2*(LeR2°2+R1*LcR2*cos(] hit 2) + ... 
RO*LcR2*cos(ThR1]+ThR2))); 

mee Dhrd*iP + MP*(-xXPd* YP + YPd* XP); 

ieotal— 4H0+-HLI+HL2 +R] +HR2+ HP; 

Hs = [HO HL] HL2 HRI HR2 TIP HTotal); 


I] 


B. AngMo2 


% Filename is "AngMo2.m" 

% This file calculates the angular momentum of the sy stem 

% Version 2 also finds the ratc of change of angular momentum 
function [Hs, Hdots} = AngMo2(Ls,Ms,CMs,Is,Q,Qdot.Qddot) 


VOUTPUL 

% Hs = 1x7 row vector of angular velocitics. The first element 1s for 
% the centerbody. The next four elements are for the left upper 
% and lower arm followed by the nght upper and lower arm. The 
% last two elements are for the payload and a total of all the 


% previous elements. [HO HL1 HL2 HRI HR2 11P 11 Total] 
% Hdots = 1x7 row vector of the change in angular velocitics. The order 


% is thc same as for Es 

% 

% INPUT: 

% Ls = 7x1 vector of lengths (m) 

% Ist element = distance from ongin to left arm mount 

% 2nd & 3rd clements wrt left arm (from shoulder toward wrist) 
% 4th element = payload length 

% Sth & 6th elements wrt nght arm (from wrist toward shoulder) 
% 7th element = distance from ngeht arm mount to ongin 


% [L0; LAS Zee RZeR IanO) 
% Ms = 6x1 column vector containing the masses (kg) 


% Ist element = mass of spacecraft centerhody 

% 2nd & 3rd elements = mass of left arm (upper arm then lower arm) 
% 4th & Sth elements = mass of nght arm (upper arm then lower arm) 
% 6th element = payload mass 


% [M0; ML1; ML2; MR1; MR2; MP] 
% CMs = 6x1 column vector containing center of mass locations 


% LeOQ- Leb): bel? Ucklvekx2: cht 

% Is = 6x1 column vector containing the moments of inertias about the 

% respective body's center of mass (kg m“%2) 

% Ist element = inertia of spacecraft centerbody 

% 2nd & 3rd clements = inertia of left arm (upper arm then lower arm) 
% 4th & 5th elements = inertia of mght arm (upper arm then lowcr arm) 


% 6th element = payload inertia 

% (10, TAT ree ar ee ae 

% Q= &x!l column vector of generalized coordinates 
% Qdot = 8x1 vector of generalized vclocities 

% Qddot = 8x1 vector of generalized accelerations 


VASA WA RARARARARASANA SA TARA RA TAMARA SLRARARATASARA SALAMA ILIAD 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
LAVA NA RASARAWASASARAWLSARARARASARASARA SAUL SA SALINE LINLE 
% Lengths (m) 


LO = Ls(1); 
= Ls(2); 
L2 = Ls(3); 
LP = Ls(4); 
R2 = Ls(5); 
= Ls(6); 
RO = Ls(7); 
% Member masses (Kg) 
MO = Ms(1); 
ML1 = Ms(2); 


MI.2 = Ms(3); 
MRI = Ms(4): 
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MR2 = Ms(5); 
MP = Ms(6), 


% Centcr of mass distances (m) 

LcO =CMs(1): 

LeL1] = CMs(2); 

Lel.2 = CMs(3); 

LcRI = CMs(4), 

LcR2 = CMs(5), 

LcP = CMs(6); Yomeasured from Icft end 


% MO] about center of mass 
10 = Is(1): 
Il. 1 =1s(2); 
11.2 = 1s(3); 
IR1 = I[s(4), 
IR2 = }s(5),; 
IP =1s(6); 


%, Coordinates (rad & m) 
ThO = Q(1). 

ThL 1 = Q(2); 

ThL2 = Q(3); 

ThhR1 = Q(4); 

ThR2 = Q(5); 

aie = Q(6); 

gee Q(/), 

ie O(S), 


% Coordinate Rates (rad/sec & m/sec) 
ThOd = Qdot(1); 

ThL 1d = Qdot(2); 

ThL2d = Qdot(3); 

ThR 1d = Qdot(4); 

ThR2d = Qdot(5); 

ThPd = Qdot(6); 

XPd = Qdoit(7),; 

YPd = Qdot(8); 


% Coordinate Accelerations (rad/sec’2 & m/scc”%2 
ThOdd = Qddot(1): 

ThL ldd = Qddot(2): 

ThL2dd = Qddot(3); 

ThR1dd = Qddot(4); 

ThR2dd = Qddot(5), 

ThPdd = Qddot(6), 

XPdd = Qddot(7): 

YPdd_ = Qddot(8), 


% Angular Momentum 

HO = ThOd*(10 + MO*Lc0%2), 

Pee Th0d*(1L 14-ML1*(LO*2+LeL1*%24+2*L0*LcL I *cos(Th 1))) + 
eves) +Millt(ieL 1) “2+L0*Lck | *cos(thie)))). 

ane DhOd*(1iL2+ML2*(L0°2+L1%24+L¢L2%2+2*1.0*1L 1 *cos(ThL1) + ... 
PE el2 *cos( Thi 2)4+-2*LO*LeL2* cosh 1+ This2))) +... 
ihibeC2aM2* del oe 2° 24-081 * cosh) +... 
Debt kecee cost hi2)+_O0*L¢eL2*cost thi, 1+Thi2))) + ... 
ine 2a (ie2+ML 2 *(LeL 2°24. i *LeL2*cos( lhl 2) + ... 
LO*LcL2*cos(ThL 1+ThL2))); 

IIR1 = Th0d*(IR1+MRI*(RO%24+LcR1%24+2*RO*LcRI *cos(ThR1))) + ... 
ThR1d*(IRI+MR1 *(LcR1I“2+RO*LcRI*cos(ThR1))): 
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HR2 = ThOd*(IR2+MR2*{RO24R 17240 CRZ°242* RO costae 
2*R1*LcR2*cos(ThR2)+2*RO*FLCR2*cos(ThRI+ThR2))) +... 
ThR1d*(IR2+MR2*(R 142+LcR2%2+RO*FR | *cos(ThR 1) +... 
2*R1*LcR2*cos(ThR2)+RO*LcR2*cos(ThR 1+ThR2))) + ... 
ThR2d*(IR2ZaMR2* (eck 272 FRc? *costinike 
RO*LcR2*cos(ThR1+ThR2))); 

HP =ThPd*IP + MP*Cxld* Pa vid), 

1] Tetal = HO + HL! +HL2+ ARI + AR2Z + oP: 

Hs = [HO HLI HL2 HRI HR2 HP HTotal]; 


% Change in angular momentum 
HOd = ThOdd*(10 + MO*LcO0%2); 
HL 1d = Th0dd*(iLI-eMiILi*(0"2 cil 24270 el) scactink yaa 
ThLidd*dL1+ML1*(LeL 1424+L0*LeL 1 *cos(Thi1))) =... 
Th0d* Th ld 2*Mel iO Ec i tsin inet ae 
Thh de 24 MET Sco* rer i sint The): 
HL2d = ThOdd*@e24Mile2*(20 2 th 24 ele2 2-2 Oe east Tinley eaes 
2* Lite 2*costihl2 a2" LOtEcEL 2 *coshel Wale) 
Thieldd*@l2+MpE2*(bl2 tcl 272 Fb 0 se cosine) a 
2*L1*EcL 2 “cos (Ihe 2) +E Os ecls2 “cosine 
Thi 2da*(le2 Mil? * (ela 2 Sel 2 coccinea 
LO*LeL2*cos(ThL 1+ThL2))) - ... 
Th0Od* Thi lid*2*ML2*(L0* 01 *sin( hile ell 22sin( ine Wile eee 
ThOd*ThL 2d*2*ML2*(L1*LcL2*sin(ThL.2)+L0*LcL2*sin(ThL.1+ThL2)) -... 
ThLid*ThL2d*2*MU2*(L1* el 2*sin( Thl2)+C0*Eclk 2 *sinG ink 1 inle7) ee 
Thi ld®2*ME2 4048 l4 singin We O*cle2 *sin( neice inl) eee 
The2d*2*ME2*(L Ich 2*sintine2 al OAc 2 cine iene ile) 
HR 1d = ThOdd*(IR1+MR1 *(RO*%24+LcR 1424+2*RO*l.cR I *cos(ThR1))) + ... 
ThR ldd*(IRI+MR1*(LcR1%2+RO*LcR 1 *cos(ThR1))) - ... 
ThOd*ThR 1d*2*MR1*RO*LcR 1 *sin(ThR1) - ... 
ThR 1d“2*MR1*RO*LcR1 *sin(ThR 1); 
HR2d = Th0dd*(1IR2+MR2*(RO%2+R 142+LceR2%2+2*RO*R 1 *cos(ThR1) +... 
2*RI*LeR2 *cos(IhR2 2 RO eee Cont et Unik? sta 
ThRidd*(R24MR2 "(el 2 eck 2e2 R08 kl cos( ikea 
2*R1*LcR2*cos(ThR2)+RO*LCR2*cos(ThRI+ThR2))) +... 
ThR2dd*(R24MR2*(UcRK 2°24: | * ek 2“ cost(hn2) 
RO*LcR2*cos(ThR1+ThR2))) - ... 
ThOd* ThR 1d*2*MR2*(ROFR 1 *sin(ThR 1)+RO*LcR2*sin(ThR1+ThR2)) - ... 
ThOd* ThR2d*2*MR2*(R 1 *LcR2*sin(ThR2)+RO*LcR2*sin(ThR1+ThR2))-... 
ThR Id*ThR2d*2*MR2*(R1 *LcR2*sin(ThR2)+RO*LcR2*sin(ThR1+ThR2))-... 
ThR 1d*2*MR2*(ROFR 1 *sin(ThR 1)+RO*LcR2*sin(ThR1+ThR2)) - ... 
ThR2d%2*MR2*(R1*LcR2*sin(ThR2)+RO*LcR2*sin(ThR 1+ThR2)); 
HPd = ThPdd*1P + MP*(-XPdd* YP - XPd* YPd + YPdd*XP + YPd*XPd), 
HdTotal = HOd + HL 1d + HL2d + HR1d + HR2d + HPd: 
Hdots = [HOd HL1d HL2d HR1d HR2d HPd HdTotal}: 


. Draw3 


% Filename is ‘Draw3.m' 
function{X, Y] = Draw3(Lengths,AngConst,AngHist Interval) 


MWWMWVVVVV%MV%%VV%%VMM%%% MMV MWWVWPWWWVVUWVWVY%Y% 
% 

% This file draws the dual arm spacecraft stick figure 

% 

% INPUTS: 

% 

% Lengths = 7x1 vector of link lengths (m) 
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% Ist element is distance from origin to left arm mount 


% 2nd & 3rd clements wrt left arm (from shoulder toward wrist) 
% 4th element 1s payload length 

% Sth & 6th elements wrt right arm (from wrist toward shoulder) 
% 7th clement ts distance from night arm mount to ongin 

% AngConst = vector of anglcs to arm mounting locations wit ccntcrbody coord 
% frame (angle for Icft arm then angle for right arm) 

% AngHist =nx6 matrix of angle histones. Each row represents a 

% specific ime. Each eolumn represents a specific joint 

% angie (cxccpt the payload angle 1s inertial) 

% Ist column 1s the ccnter body angle 

% 2nd & 3rd columns arc the left arm shoulder and elhow 

% 4th & Sth columns arc the right arm shoulder and clhow 

% 6th column 1s the payload (this angle 1s mertial) 

% Interval = plot cvcry "interval'th" time 

% 

% 

OUTPUTS: 

% 


% X = vector history of joint x coordinates 

% Y = vector history of joint y coordinatcs 

% X & Y treat the system as a closed chain beginning at the centcrbody origin, 

% outward along the left arm, across the payload, inward along the nght arm, 

% and back to the origin. 

% 

MMAVADVVWAMVMVVDAMMMMMMMMM MMMM AMMVWMMUAMAVN% 


{Times,dummy | = size(AngHist); 

Links = length(Lengths); 

X(1,1) = 0; 

Y(1,1) = 90; 

% Convert the joint angles to inertial angles and reorder them for closed chain usc 
NAng(:,1) = AngHist(:,1) + AngConst(])*ones(Times.] ). 
NAng(:,2) = NAng(:,1) + Angl list(:,2); 

NAng(:,3) = NAng(:,2) + AngHist¢:,3); 

NAng(:,4) = AngHist(:,6); 

NAng(:,7) = AngHist(:, 1) + AngConst(2)*ones(Times,]) + pi; 
NAng(.,6) = NAng(:,7) + AngHist(: 4), 

NAng(:,5) = NAng(:,6) + Angl list(:,5); 


p=, 
While p <= Times 
for gq = 1:Links 


Lastx = 0; 
Lasty = 0; 
forr = 1:q 


Lastx = Lastx + Lengths(r)*cos(NAng(p.r)): 
Lasty = Lasty + Lengths(r)*sin(NAng(p,r)); 
end 
(Q+1.p) = Lastx: 
Y(qt+1,p) = Lasty; 
end 
p =p + Interval; 
end 
X = [X(1:Links,:); X(2,:); X(Links,:); X(Links+1,:)]; 
ey (1:Links,:). Y(2,:); Y(Links,:),; Y(Links+1,:)], 


% Plot the Final Case 


for q= 1:Links 
Lastx = 0; 
Lasty = 0, 


ks 


for r= 1:q 
Lastx = Lastx + Lengths(r)*cos(NAng(Times,r)); 
Lasty = Lasty + Lengths(r)*sin(NAng(Times,r)); 
end 
XFinal(g+1,1) = Lastx; 
YFinal(qt+1,1) = Lasty; 
end 
XF inal = [XFinal(1:Links,:),; XFinal(2,:); XFinal(Links,:). XFinal(Links+1,:)]; 
YFinal = [YFinal(1:Links,:); YFinal(2,:); YFinal(Links,:); YFinal(Links+1,:)]; 


clg; 

axis(‘square’) 

plot(X, Y,'-"|XFinal, YFinal,'-',XFinal, YFinal,'x', X(:,1), YC, 1),'o'); 
xlabel('X (m)');ylabel('Y (m)'); 

ax1s(‘normal') 


Eqn2 


% Filename is ‘'Eqn2.m' 

% Differential Equations for numeneal integrator 

function [Xdot,U,TorqRef,Aqdot,J,Res, LHS,RHS,Delq] =... 
Eqn2(T,X,Ls,Ms,CMs,Is,BoundC,Gains,X{Des, Wu. We,Coef,ConstMat) 


% OUTPUT: 

% xdot = derivatives of state veetor at time T 

% U =eolumn vector of actual torques commanded at time T arranged 
% as [U1]; U2; U6; U5] where the number denotes the joint 


% associated with that torque 

% TorqRef = column vcctor of reference torques that should be applied 
% at time T if the motion followed the referenee maneuver exaetly. 
% These are arranged in the same order as U. 


% Res = column vector of residuals after EOM are evaluated with the 

%  ealculated reference torques. (Residuals should be zero). 

% Aqdot = eolumn veetor of A*qdot. This is a test to see if the 

% constraint equation (A*qdot = 0) 1s satisfied. 

% LHS = eolumn veetor of the EOM left hand side (LIIS = M*qddot + GTilda) 
% RIS = eolumn veetor of the EOM nght hand side (RITS = BTilda*u) 

% 
% INPUTS: 

% T = time (see) 

% State Vector, X, 1s defined as follows: 

%  X1 = Theta O (rad) 

%  X2 =ThetaL! (rad) 

%  X3 = Theta L2 (rad) 

% 4 = Theta RI (rad) 

% 5 = Theta R2 (rad) 

% X6 = Theta P (rad) 

% &7 =X eomponent of Payload Center of mass position (m) 

% &8& = Y component of Payload Center of mass position (m) 

%  X9 = Theta 0 Dot (rad/see) 

%  &10= Theta LI Dot (rad/see) 

% X11 = Theta L2 Dot (rad/sec) 

%  X12= Theta R1 Dot (rad/scc) 

% X13 = Theta R2 Dot (rad/see) 

% X14= Theta P Dot (rad/see) 

% &I5=X component of Payload Center of mass veloeity (m/sec) 

% %&16= Y component of Payload Center of mass velocity (m/sec) 

% X17 = integral of the absolute value of the centerbody disturbanee torque 
% X18 = integral of the centcrbody disturbance torque squared 
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% |.s = 7x1 vector of lengths (m) 


% Ist element = distance from ongin to left ann mount 

% 2nd & 3rd elements wrt left arm (from shoulder toward wrist) 

% Ath element = payload length 

% Sth & 6th elements wrt right arm (from wrist toward shoulder) 

% 7th element = distanee from nght arm mount to origin 

% eG ee 2 PZ ORI RO| 

% Ms = 6x1 column vector containing the masses (kg) 

% Ist element = mass of spacecraft centerbody 

% 2nd & 3rd elements = mass of left arm (upper arm then lower arm) 
% 4th & Sth elements = mass of night arm (upper arm then lower arm) 


% 6th element = payload mass 

% (MO; ML1,ML2; MRI; MR2. MP] 

% CMs = 6x1 column vector containing center of mass locations 
piece. Le, LeL2; LeRI; LeR2; LeP] 

% 1s = 6x1 column vector containing the moments of incrtias about the 
% respective body's center of mass (kg m*2 

% Ist element = inertia of spacecraft centerbody 

% 2nd & 3rd elements = inertia of left arm (upper arm then lower arm) 
% 4th & Sth elements = inertia of night arm (upper arm then lower arm) 
% 6th element = payload incrtia 

pope lO. WLI: 12; IR1. IR2; IP} 

% BoundC = boundry conditions for the problem. The first column 

% contains the initial x and y component of points Q & P 

% respectively, the x componcnt of the nght arm base, the 

% problem start time, and the simulation stop timc. The second 

% column contains the x and y component of points Q & P 

% respectively, the x componcnt of the right arm basc, the 

% stop time for the ideal reference maneuver, and a flag to 

% activate or deactivate the controller. The ongin for the 

% x andy components is the base of the Icft arm. 

% Wu = 6x6 control torque cost weighting matrix 

% We = 8x8 constraint cost weighting matrix 

% Gains = 1x2 column vector of controller gains. The first value is 
% for position gains and the second value is for velocity 

% gains. 

% XfDes = column vector containing desired values for the angles at 
% the conclusion of the maneuver. These arc the same angles 

% the reference mancuver is trying to create. They are arranged 

%  as[ThOf, ThL1f; ThL2f, ThRIf, ThR2f; ThP!. 


WWW MMVMMMMMMMMMM MMMM AZ% 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
MMMM WMV VAWMMVMWAAMDAM/%% 


ThO = X(1); 
ThL 1 = X(2); 
ThL2 = X(3); 
ThR] = X(4); 
ThR2 = X(5); 
ThP = X(6),; 
Xc = X(7); 
Ye = xX(8); 
ThOd = X(9); 


ThL Id = X(10); 
ThL 2d = X(11); 
ThRId = X(12); 
ThR2d = X(13); 
ThPd = X(14); 
Xed = X(15); 

Yed = X(16); 
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% Arms mount locations wrt spacecraft centerbody coordinate frame (rad) 
Pp 


ThLO = BoundC(5,1); ThRO = BoundC(5,2); 


% Stop Times 
TIR = BoundC(6,2), % Reference Torquc stop time (sec) 
TIC = BoundC(7,1); % Controller stop time (sec) 


% Controller Flag 
ContFlag = BoundC(7,2); 


% Constraints Matrix Flag 
AMatF lag = BoundC(8, 1); 


% Centerbody Reaction Wheel Flag 
WheelFlag = BoundC(8,2); 


% Kinetic Energy Test Flag 
KEFlag = BoundC(11,1); 


% Inverse Kinematics Bypass Flag 
ByPass = BoundC(11,2), 


% Torque selection if bypass 1s enabled 
TorgF lag = BoundC(12,1); 


% Maximum torque from reaction wheel 
TorqC ap = BoundC(13,1); — % Limit cnabled 
TorqMax = BoundC(13,2); % Limit amount 


% Controller Gains: 
Gpos = Gains(1); 
Gvel = Gains(2), 


MAMMA MMM%MM%%%% 

%% CALCULATIONS %% 

YAVWMVVMMMM%%%%% 

% EOM: M*qddot + dV/dq + G = Qf + A'*Lam 

% M is mass matrix 

% qddot is column vector of gencralized coordinate accelerations 
% dV/dq is the partial derivative of the potential function with 

% respect to the generalizcd coordinates. This term 1s zero for 
% this problem because all motion 1s in the honzontal plane (there 
% isno change in potential energy causcd by the motion) 

% G is a column vector which 1s a function of q and qdot 

% Qf are generalized forces caused by joint torques 

% A‘ 1s transpose of constraints matrix 

% Lam are Lagrange multipliers 


%H)™%M%%H%M%MMM%M%MMMM%M%% 

%%, State Vector & Derivative %% 
%YH™%MMMMMNMMMNMMM%% 

QO = [ihO) Th) The? hk wih? ie rc) 

Qdot = [ThOd; ThL 1d, ThL2d; ThR1d; ThR2d: ThPd; Xed; Yed]; 


MYYM%AV%V%% 
%% Matrices %% 
WHUVM>V%MMM%N% 
AngConst = [ThLO; ThRO}]; 
if AMatFlag 
(M,G,A,Adot,B] = MatxFix(Ls,Ms,CMs,]s,Q, Qdot,AngConst); 
elsc 
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[M,G,A,Adot,B] = Matx(Ls,Ms,CMs,Is,Q,Qdot,AngConst), 
end 


if WheelF lag 
B7 = [1; 0; 0; 0; O; 0; 0; O]; 
B = [B7 B}, 

end 


if ByPass % If true, then bypass ealeulating torques using inverse 
% kinematies. This braneh of logic is a verifieation test 
% during program development and is not intended for regular 
% use onee the program is eheeked out. 
if TorqF lag == 0 
U =zeros(6,1); J=0; 
else 
if TorqF lag == | 
U = [-0.01, 0; 0; 0; 0; OJ, J = 0; 


else 
U = [-0.01,; 0; 0; 0.01, 0; OJ; J =0; 

end 
end 
if WheelF lag 

U = [0; UJ; 
end 

else % Normal program flow to find control torques 


WMV VM%M%M% 
%% Torques %% 
VMMVWV%VUNM%N% 
ifT <=TfR, % Get the appropnate torque and angle values 
% from the referenee maneuver ealculations 
{TorqRef, QRef, QdotRef, Aqdot, J, C1] Ref, C2Ref, C3Ref] = ... 
Ref2(Ls,Ms,CMs,Is,BoundC,T, Wu, We,Coef,ConstMat). 
else % Simulation is longer than ideal referenee maneuver 
% Use no referenee torques 
% Use the desired final values as references 
TorqRef = [0, 0. 0; 0; 0; OF; 
QRef(1) = XfDes(1); 
QRef(2) = XfDes(2); 
QRef(3) = XfDes(3); 
QRef(4) = XfDes(4); 
QRef(5) = XfDes(5); 
QRef(6) = XfDes(6), 
QRef(7) = XfDes(7), 
QRef(8) = XfDes(8); 
QdotRef(1) = XfDes(9), 
QdotRef(2) = X{Des(10); 
QdotRef(3) = XfDes (11); 
QdotRef(4) = XfDes(12), 
QdotRef(5) = XfDes(13); 
QdotRef(6) = XfDes(14); 
QdotRef(7) = X{Des(15), 
QdotRef(8) = XfDes(16); 
if WheelFlag 
TorqRef = [0; TorqRef]; 
end 


% Matrices 
if AMatFlag 
[MRef,GRef,ARef,AdotRef] = MatxFix(.s,Ms,CMs|Is,... 
QRef,QdotRef,Ang Const). 
else 
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[MRef,GRef, ARef,AdotRef] = Matx(Ls,Ms,CMs,Is,QRef,QdotRef,... 
AngConst), 
end 
BRef = B; 
PtiRef = ARef*inv(ARef*inv(MRef)*ARef), 
C1Ref = inv(MRef)*(eye(M) - PtIRef*ARef*inv(MRef))*BkRef, 
C2Ref = -inv(MRef)*Ptl Ref*¥AdotRef, 
C3Ref = inv(MRef)*(PtlRef*A Ref*inv(MRef) - eye(M))*GRef: 
end 


if ContFlag % Controller 1s on 

Delg = Q - QRef’; 
Delqdot = Qdot - QdotRef; 
% Controller ealculations 
Pt] = A'*inv(A *inv(M)*A‘); 
Cl = inv(M)*(eye(M) - Ptl*A*inv(M))*B, 
C2 = -inv(M)*Ptl *Adot; 
C3 = inv(M)*(Ptl *A*inv(M) - eye(M))*G; 
F2 = Gpos * Delg; 
F2 = [F2(1:6); 0; Of; 
Kv = Gvel * eye(M); 
Kv(7,7)=0; Kv(8,8)=0; 
Pt3 = pinv(C 1). 

% Pt3 = inv(C1'*C1)*C1'; % Resulted in poorly conditioned matrix 


MMMM VVMMVMMVMVDMN%%VY% 

%% Complete Lyapunov Controller %% 

MUWAMVMVMMVMMMVMANMUV%M%%% 
U = Pt3*(-Kv*Delqdot + C1lRef*TorgRef - (C2*Qdot - C2Ref*QdotRef’) - ... 

(C3 - C3Ref) - F2); 

MAVDVMAMMMUMMMMM%MV%V% 

%% Simplified Lyapunov Controller %% 

%% (removes referenee torques and %'%o 

%% assumes C2 and C3 terms are small) %% 

MAMMA VVAMMV%/0%%0% 70% 

% Kp = Gpos * eye(M); 

% Pt3 = pinv(C1 Ref), 

% U = Pt3*(-(Kv+C2Ref)*Delqdot - Kp*Delq) + TorqRef; 


else % Controller 1s off 
U = TorgRef; % Don't adjust torques from referenee mancuver 
Delg = 999*ones(8,1); % Dummy value for trajeetory error 

end % End of Control Loop 

if WheelF lag 
J = abs(U(1 )); 

else 
J=0, 

end 

end % End of Kinetie Energy Test Conditional 


if TorqCap  % Upper limit on wheel torque enabled? 
if abs(U(1)) > TorqMax 


if UQ1)>0 
U1) = TorqMax; 
else 
U(1) = -TorqMax; 
end 
end 


end 


MWWVD%%%Y% 
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%% Qf %% 

MWWAV%% 

% Qf=B*u These are the generalized forces 
er—B * U: 


MMIMAVMVMDVVMMMV%%M% 

%% Lagrange Multipliers %% 

MMVVVMVMMVMM%%% 

% EOM: M*qddot + dV/dqg + G = Of + A'*Lam 

% Solving the EOM for qddot gives: qddot = inv(M)*(Qf + A'*Lam - G) 
% Differentiating the Pfaffian form of the constraint equations 

% results in: Adot*qdot + A *qddot = 0. 

% Substitution of the expression for qddot into the previous equation 
% permits solving for Lam: 

% Lam = inv(A*inv(M)*A')*(A*inv(M)*(G-Qf) - Adot*qdot); 
Lam = inv(A *inv(M)*A')*(A *inv(M)*(G-Qf) - Adot*Qdot): 


MWWWAMWVWWAMVAMM%%Y% 
%% Putting it all together %% 
MWMV%V%V%%M%M%%%%% 
Qddot = inv(M) * (Qf + A'*Lam - G), 


[l1s, Hdots] = AngMo2(Ls,Ms,CMs,Is,Q,Qdot,Qddot); 
% Change in total angular momentum 

Hd = Hdots(7),; 

J = [J; Hd]; 


% Assemble derivative of state vector for integrator 
Xdot = [Qdot, Qddot; IC); (IC1))%2]; 


WMDVVVVVAVAM%%%% 
%% Troubleshooting Info %% 
WWWVAVVVMM%%0%%%M% 
Aqdot = A*Qdot; 

LEIS = M*Qddot + G; 

RHS = Of + ALam; 

Res = LHS - RHS; 


. fminu2 


function [x,OPTIONS] = fminu2(FUN,x,OPTIONS, GRADFUN,P1,P2.P3,P4,P5,P6.... 
P78 FOP) 

%FMINU Finds the minimum of a function of several variables. 

% X®=FMINUCFUN'XO) starts at the matnx XO and finds a minimum to the 

% function which 1s described in FUN (usually an M-file: FUN.M). 

% The function 'FUN' should return a scalar function valuc: F=FUN(X). 

% 

% X=FMINUCFUN',X0,OPTIONS) allows a vector of optional parameters to 

% be defined. OPTIONS(1) controls how much display output is given, set 

% tol for atabular display of results, (default 1s no display: 0). 

%  OPTIONS(2) 1s a measure of the precision required for the values of 

%  Xatthe solution. OPTIONS(3) is a measure of the precision 

% required of the objective function at the solution. 

% For more information type HELP FOPTIONS. 

% 

%  X=FMINUCFUN',X0,OPTIONS,’'GRADFUN’) enables a function’GRADFUN' 

% tobe entered which returns the partial derivatives of the function, 

%  df/dX, at the point X: gf = GRADFUN(X). 

% 


I) 


% The default algorithm is the BF GS Quasi-Newton method with a 
% mixed quadratic and cubic line search procedure. 


% Copyright (c) 1990 by the MathWorks, Inc. 
% Andy Grace 7-9-90. 


Y, ------------ Initialization---------------- 
XOUT=x(:); 
nvars=length(XOUT), 


evalstr = [FUN], 
if ~any(F UN<48) 
evalstr=[evalstr, '(x']; 
for 1=1] :nargin - 4 
evalstr = [evalstr,',P’ num2str()]; 
end 
evalstr = [cvalstr, ')’]; 
end 


if nargin < 3, OPTIONS={[]; end 
if nargin < 4, GRADFUN=T]; end 


if Iength(GRADFUN) 
evalstr2 = [GRADFUN], 
if ~any(GRADFUN<48) 
evalstr2 = [evalstr2, '(x'], 
for 1=]:nargin - 4 
evalstr2 = [evalstr2,’,P’ num2str(1)]; 
end 
evalstr2 = [evalstr2, ')']; 
end 
end 


f = eval(evalstr); 

n= length(XOUT), 

GRAD=zeros(nvars, 1); 

OLDX=XOUT; 

MAT X=zeros(3,1); 

MATL=[f;0;0]; 

OLDF=f;: 

FIRSTF=f; 

[OLDX,OLDF,HESS OPTIONS |=optint(XOUT,f,OPTIONS), 
CHG = le-7*abs(XOUT)+ le-7*ones(nvars, |); 
SD = zeros(nvars, |); 

diff = zeros(nvars, 1); 


OPTIONS(10)=2, % Itcration count (add | for last evaluation) 
status =- 1; 


while status ~= | 

% Work Out Gradicnts 

if ~length(GRADFUN) | OPTIONS(9) 
OLDF=f; 

% Finite difference perturbation levels 

% First check perturbation level 1s not less than search direction. 
f = find(10*abs(CHG)>abs(SD)); 
CHG(/ = -0.1*SD(H; 

% Ensure within user-defined limits 
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CHG = sign(C]HG+eps). *min(max(abs(CHG),OPTIONS(16)),OPTIONS(17)); 
for gcnt=] :nvars 
XOUT (gent, |J=XOUT(gent)+Cl IlG(gent); 
OPTIONS(10)=OPTIONS(10)+1; 
disp(While Loop Itcration in Progress’); 
disp([‘Iterations: ',num2str(OPTIONS(10))}); 
disp(['Allowable:  'jnum2str(OPTIONS(14))]); 
x(:) = XOUT, f = cval(evalstr). 
GRAD(gent)=(f-OLDF)/(CHG(gent)). 
if f < OLDF 
OLDF=f, 
else 
XOUT(gent)=XOUT(gent)-CHG(gent): 
end 
cnd 
% Try to sct difference to Ic-8 for next itcration 
CHG = Ic-8./GRAD, 
t= OLDF; 
% OPTIONS(10)=OPTIONS(10)+nvars; 
% Gradient check 
if OPTIONS(9) == 
GRADFD = GRAD. 
x()=XOUT,;, GRAD = eval(evalstr2); 
graderr(GRADFD, GRAD, evalstr2); 
OPTIONS(Y) = 0; 
end 


else 
OPTIONS(11)=OPTIONS(11)+1, 
x()=XOUT,; GRAD = eval(evalstr2); 


if status == - 

SD=-GRAD,; 

FIRSTF={; 

OLDG=GRAD, 

GDOLD=GRAD"*SD,; 

% For initial step-sizc guess assume the minimum 1s at zero. 

OPTIONS(18) = max(0.01, min([1,2*abs(f/GDOLD)})), 

if OPTIONS(1)>0 

% disp([sprintf(’‘%5.0f %12.32 %12.3g 'OPTIONS(10),f.... 
OPTIONS(18)),sprintf{(‘%12.3g ';GDOLD)}); 

end 

XOUT=XOUT+OPTIONS(18)*SD; 

status=4; 

if OPTIONS(7)==0; PCNT=1,; end 


------------- Direction Updatc------------------ 
gdnew=GRAD'*SD; 
if OPTIONS(1)>0, 
num={[sprintf{(‘%5.0f %12.3g %12.3g ,OPTIONS(10),f£,0PTIONS(18)).... 
sprint{('%12.3g ',gdnew)]; 
end 
if (gdnew>0 & f>FIRSTF)|~finite(f) 
% Case 1: New function ts bigger than last and gradient w.r.t. SD -ve 
%  ...interpolate. 
how="inter'; 
[stepsize]=cubicil (,FIRSTF,gdnew,GDOLD,OPTIONS(18)): 
if stepsize<Olisnan(stepsizc), stcpsize=OPTIONS(18)/2; how='C If‘; end 
if OPTIONS(18)<0.1 &OPTIONS(6)==0 
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if stepsize*norm(SD)<eps 
rand(‘normal’) 
stepsize=rand( 1); 
how="RANDOM STEPLENGTIT'. 
status=0; 
else 
stepsize=stepsize/2; 
end 
end 
OPTIONS(18)=stepsize; 
XOUT=OLDX; 
clscif f<FIRSTF 
[newstep,fbest] =cubici3 (f,FIRSTF, gdnew,GDOLD,OPTIONS(18)). 
sk=(XOUT-OLDX)*(GRAD-OLDG); 
if sk>1e-20 
% Case 2: New function less than old fun. and OK for updating HESS 
% .... update and calculate new direction. 
how=". 
if gdnew<0 
how="'incstep’; 
if newstep<OPTIONS(18) 
newstep=2*OPTIONS(1I8)+1e-5. 
how=[how,' IF'J; 
end 
OPTIONS(18)=min([max({2,1.5*OPTIONS(18)]), 1 +sk+abs(gdnew)-+... 
max({0,OPTIONS(18)-I]), (1.2+0.3*(~OPTIONS(7)))*abs(newstep)]); 
else % gdnew>0 
if OPTIONS(18)>0.9 
how="'int_ st’; 
OPTIONS(18)=min([1,abs(newstep)]): 
end 
end %if gdnew 
[HESS,SD]=updhess(XOUT,OLDX,GRAD,OLDG,HESS,OPTIONS), 
gdncw=GRAD'*SD; 
OLDX=XOUT; 
status=4; 
% Save Variables for next update 
FIRSTF=f; 
OLDG=GRAD, 
GDOLD=gdnew, 
% If mixed interpolation set PCNT 
if OPTIONS(7)==0, PCNT=I,; MATX=zeros(3,1), MATL(1)=f, cnd 
clscif gdnew>0 %osk<=0 
% Case 3: No good for updating HESSIAN .. interpolate or halve stcp length. 
how='inter_st'; 
if OPTIONS(18)>0.01 
OPTIONS(18)=0.9*newstep; 
XOUT=OLDX;, 
end 
if OPTIONS(18)>1, OPTIONS(18)=1; end 
else 
% Increase step, replace starting point 
OPTIONS(18)=max([min([newstep-OPTIONS(18),3]),0.5*OPTIONS(18)]); 
how="'1ncst2': 
OLDX=XOUT; 
FIRSTF=f, 
OLDG=GRAD, 
GDOLD=GRAD"*SD; 
OLDX=XOUT, 
end % if sk> 
% Case 4: New function bigger than old but gradient in on 
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% ..reduce step length. 
else Yogdnew<O0 & F>FIRSTEF 
if gdnew<O&f>FIRSTF 
how='red_step’; 
if norm(GRAD-OLDG)<le-10; HESS=eye(nvars), end 
if abs(OPTIONS(18))<eps 
rand(‘normal') 
SD=norm(SD)*rand(SD) 
OPTIONS(18)=abs(rand(1))* le-6; 
how='RANDOM SD'; 
else 
OPTIONS(18)=-OPTIONS(18)/2; 
end 
XOUT=OLDX; 
end %gdnew>0 
end % if (gdnew>0 & F>FIRSTF)|~finite(l) 
XOUT=XOUT+OPTIONS(18)*SD; 


if OPTIONS(1)>0 

% disp({num,how]) 

end 

end %---------- End of Direction Update------------------- 


% Check Termination 
if max(abs(SD))<2*OPTIONS(2) & (GRAD'*(SD)) < 2*OPTIONS(3) 
if OPTIONS(1) > 0 
disp(");disp("):disp("); 
disp(");disp("):disp("); 
disp(‘Optimization Terminated Successfully’), 
% disp(‘Gradient less than options(2)’): 
disp({[' NO OF ITERATIONS=", num2str(OPTIONS(10))}); 
end 
status=1; 
elseif OPTIONS(10)>OPTIONS(14) 
if OPTIONS(1)>=0 
disp(");disp(");disp("), 
disp(");disp("):disp("); 
disp(‘Waming: Maximum number of iterations has been cxceeded’); 
disp(' _—s-- inerease options(14) for more iterations.") 
end 
status=1; 
else 


% Line search using mixed polynomial interpolation and extrapolation. 
if PCNT~=0 
while PCNT > 0 
OPTIONS(10)=OPTIONS(10)+1; 
disp(");disp("):disp("); 
disp(‘Termination Check in Progress’), 
disp({‘Iterations: ';num2str(OPTIONS(10))]): 
x(:) = XOUT; 
f = eval(evalstr), 
{[PCNT,MATL,MATX,steplen,f, how |=searchq(PCNT,f,OLDX,... 
MATL,MATX,SD,GDOLD,OPTIONS(18), how), 
OPTIONS(18)=steplen; 
XOUT=OLDX+steplen*SD; 
if abs(steplen) < le-6, PCNT=0, status=1; end 
end 


else 
x()=XOUT,. 
OPTIONS(10)=OPTIONS(10)+1; 


125 


disp(");disp(");disp("); 
disp(‘Termination Check in Progress’), 
disp(['Itcrations: 's;num2str(OPTIONS(10))}); 
f = eval(cvalstr); 
cnd 
end 
end 


x()}=XOUT; 
disp(");disp("):disp("); 
disp("),disp(");disp(’). 
disp(").disp(");disp(’); 
disp(‘Final Evaluation in Progress’), 
‘= eval(evalstr), 

fi] hiksik 
OPTIONS(8) = FIRSTF; 
x(=OLDX; 

elsc 

OPTIONS(8) = f; 

end 


MainMin 


% Filename is "MainMin.m" 

% This is the routine used by "MainOpt.m" to optimize the reference 
% trajectory polynomial coefficients. It is a scaled down version 

% of the dual arm spacccraft program, "Main2.m". This version does 
% not intcgratc the state variables not include a lLLyapunov controller. 
% The only integration that does take placc 1s the optimization cost 
% function. 


function [JOpt] = MainMin(UpCoef,Const Mat,Ilags) 


Yoclg;clear; 

format compact;format short, 

k = Iength(UpCoef); 

A543 = inv(ConstMat(:,k+1:k+3))*({1; 0; O] - ConstMat¢:,1:k)*UpCoef’), 
Cocf = [UpCoef’; A543]; % Reference trajectory polynomial cocfficients 


% Reference Maneuver Start and Stop Times 


TU 20: 
TIR = 10; 
T£C = 10; 


MetaFlag = Flags(1); 
ContFlag = Flags(2), 
PertFlag = Flags(3); 
AMatFlag = Flags(4); 
WheelF lag = Flags(5); 
EOMFlag = Flags(6), 
PinvFlag =Flags(7); 
KEFlag = Flags(&), 
OutFlag = Flags(9); 
Trace = Flags(10); 
SymFlag = Flags(11); 
ByPass = Flags(12); 
TorqFlag = Flags(13), 


Tol = le-6, % Integration tolerance 
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R2D = 180/pi, % Conversion faetor from radians to degrees 


% Lengths (m) 


BOS 075, % Onegin to Icft shoulder 
L] =0.5; % Left upper arm 

12 =0.5; % Left forearm 

PP =()5. % Payload 

R2=0.5, % Right forcarm 

Fel = 0.5; % Right upper arm 


RO = sqrt(2*0.75%2): % Ongin to nght shoulder 
Peete LL), £2; LP, R2; RI; RO}; 


% Member masscs (kg) 
MO =5, 

ML1 = 1, 

ML2 = 1; 

MR] = 1; 

MR2= 1; 

MP =1,; 


Ms = [MO; ML1; ML2; MR1; MR2; MP}; 


% Center of mass distances (m) 


LcO =0; 

ciel = 0.25: 
LeL2 = 0.25, 
eR) = 0.25. 
ek? — 0.25. 
LeP =0.25; 


Seis 1ec0, UcL 1; LcL2; LeR1; LcR2; LceP]; 


% MOI about center of mass: Ic = (1/]12)*(mass)*(length)*2 
10 = MO; 

%10 = 0; 

Meee 1/2) * M1 * L142; 

eee (1/12) * ML2 * L2%2; 

eee — (1/12) * MRI * R1%2; 

eee — (1/12) * MR2 * R2“2; 

Nee 1/12)* MP * LP*%2: 

fe 10; IL 1; IL2; IR1; IR2; IP]; 


% Nominal initial and desired final locations of payload 
% Point Q 1s at wrist of left arm 

% Point P is at wrist of right arm 

OsOn—0.125; QyOn= 1.5; 

PxOn = 0.625; PyOn= 1.5; 

Qxf =0.125; Qyf = 1.0; 
Pxf =0.125; Pyf =1.5, 
% Nominal initial and desired final locations of centerbody 
ThOO = 0; 

ThOf = O/R2D, 


% Arms mount locations wrt centerbody coordinate frame (rad) 
ThLO = pi/2; 

ThRO = pi/4; 

AngConst(1) = ThLO; 

AngConst(2) = ThRO; 


% Symmetric geometry to center arms and test kinetic energy 


if SymF lag 
ThLO = 3*pi/4; 
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AngConst(1) = ThLo; 

RO=LO; % Origin to nght shoulder 
Ls(7,1) = RO; 

QxOn = -0.25, QyOn = 1.2; 

PxOn = 0.25; PyOiv= eZ: 


end 

BoundC(1,1) = QxOn; BoundC (1,2) = QyOn; 
BoundC (2,1) = PxOn; BoundC(2,2) = PyOn: 
BoundC(3,1) = Oxf; BoundC(3,2) = Qvf. 
RBoundC(4,1) = Pxf; BoundC(4,2) = Pyf. 
BoundC(5,1) = ThLO; BoundC(5,2) = ThRO, 
BoundC(6,1) = TO; BoundC (6,2) = TiR; 
BoundC(7,1) = T£C; BoundC(7,2) = ContFlag: 
BoundC(8,1) = AMatFlag; BoundC(8,2) = Wheel lag. 
BoundC(9,1) = Thoo; BoundC(9,2) = Thof, 


BoundC(10,1)= EOMFlag; BoundC(10,2)= PlInvilag: 
BoundC(11,])= KEF lag, BoundC(11,2)= By Pass; 
BoundC(12,1)= TorqF lag; 


% Weighting Matrices 
% Control torques are ealeulated to minimize the following eost function: 
% J=0.5*(u'*Wu*u + (A'*Lam)'*We*(A'*Lam)) 


if WheelF lag 
Wu = eye(7),; % Control Torque Weighting 
else 
Wu = eye(6); 
end 
%if Wheel lag 
% Wu =zeros(7,/); 
“else 
% Wu = zeros(6,6), 
“end 


%Wu(4,4)=1e5: 

%Wu(7,/)=1e5; 

%Wu(2,2)=l1e10; 

%Wu(5,5)=1e10; 

We = zeros(8,8), % Constraint Force Weighting 
YWe = eye(8); 


MRwMMMMMAAMMMMMMM%% 
“’%% INITIAL CONDITIONS %% 
MMMVDMVAAHMMMM%% 

% Desired Initial Payload Parameters 
ThPO = atan2(PyOn-Qy0n,PxOn-QxOn); 
XCO = 0.5 * (PxOn + QxOn); 

YeO = 0.5 * (PyOn + QyOn); 


Qx0 = QxOn; 
Qy0 = QyOn; 
PxO = PxOn; 
PyO = PyOn, 


% Initial State 
XO = 0; 


MUVMMMMMMMM%%N% 
%% INTEGRATION %% 
MMMM %%N% 
% RefMin2 uses change in angular momentum to find wheel command torque 
[T,JInt,J] = odemin(RefMin2',T0,T&R,X0,Tol, Traee.Ls,Ms,CMs,Is,BoundC,... 
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Wu, We,Coef,ConstMat); 
% Optimization eost funetion 1s integral of J 
k = length(T), 
JOpt = JInt(k), 
%JOpt = max(abs(J)); 


. MainOpt 


% Filename is “"MainOpt.m" 

% This routine optimizes the dual arm spaeeeraft cost funetion 
% by changing the polynomial eoefficients whieh deseribe the 
% reference trajectory. It calls "Main2.m" 


%clear 

ele 

home 

format compact 
format short 


UpCoef0 = [0]; % Starting Guess 

%UpCoefO = UpCoef; % Use last values for starting guess 

k = length(UpCoef0); 

options = []; % Default values 

options(1) = 0; % Display dunng optimization eycle: 1=On, 0O=Off 
options(14) = 100*k; % Maximum number of iterations 


MMMMMMMMAIMMMMMMMMM%MMMMMM%MMMM%MWV%Y% 
%% Flags during optimization %% 
MWVMMMM%MVMMVMMMM%MM%MMM%MMM%MMMMMMM%M%%% 
Metal lag = 0; % Creates metafile named "main.met" 
ContFlag = 0; % Controller Status Flag: 1=On; O=Off 
Pertklag =0; % Perturbation Flag (O=no perturbation, | =perturbation) 
% The perturbation 1s to eheck the controller by 
% disturbing the actual initial state away from nominal. 
% The referenee torques are based on nominal. 
AMatFlag = 0; % Size of A matrix: 0 = 4x8 (Free Centerbody ) 
% 1 = 5x8 (Fixed Centerbody ) 
WheelF lag = 1, % Centerbody Reaetion Wheel (1=On, O=Off) 
EOMFlag = 8; % Speeifies number of cost funetion eonstraint equations 
% 3 = only payload equations 
% 5 = only spaceeraft equations 
% any other value = all 8 equations 
PlnvFlag = 1, % Psuedo-Inverse Flag (for use in finding reference torques) 
% 1 = Use psuedo-inverse 
%  O= Use inverse 
KEFlag=0; % KE Test Flag 
% 1 =Nonzero veloeity initial eonditions 
%  O= Zero velocity initial conditions 
OutFlag=0; % Output Flag 
% 1 =Display output 
% O= Don't display output 
Traee = |; % Observe integration 
%  1= Observe 
%  O=Don't observe 
OptFlag =0; % Optimization Flag 
% 1 = Perform optimization 
% Q= Don't perform optimization 
SymFlag = 0, % Symmetric Geometry Flag 
%  1|=Symmetrie geometry 


We 


%  O=Nonsymmetrie geometry 
ByPass =0, % Torque ealeulation bypass flag 
% |= Bypass 
%  O= Use mverse kinematics 
Torgklag= 0, % Torques to use if bypass enabled 
% O=No Torques (Dnft) 
% |= One Shoulder Torque 
%  2=Symmetrie Shoulder Torques 
TorqCap =0; % Maximum limit on wheel torque 
%  1=Enabled 
% O0= Disabled 
TorqMax = 0.075;% Limit on wheel torque if TorqCap enabled 
Flagsl(1) = MetaF lag; 
Flags|(2) = ContFlag; 
Flags! (3) = PertF lag: 
Flags1(4) = AMatFlag. 
Flags1(S5) = WheelFlag; 
Flags1(6) = LOMF lag; 
Flags1(7) = PInvF lag; 
Flags1(8) = KEF lag; 
Flags1(9) = OutF lag; 
Flags1(10)= Trace; 
Flags1(11)= SymFlag; 
Flags1(12)= ByPass; 
Flags1(13)= TorgF lag; 
Flags1(14)= TorqCap; 
Flags1(15)= TorqMax; 
WWWVAVUVVWVVVVV%YOY 
%% Flags after optimization %% 
ASA RARLUL SA RANA RASA RANA SARA 
Flags2 = Flags], 
Flags2(1) = 0; % MetaFlag: 1=On, 0O=Off (File is "main.met") 
Flags2(2) = 1; % Controller Flag: = 1=On, O=Off 
Flags2(3) = 0; % Perturbation Flag: 1=On, 0=Off 
Flags2(5) = 1; % Wheel Flag: 1=On, 0=Off 
Flags2(8) = 0; % Kinetie Energy Flag: 1=On, O=Otf 
Flags2(9) = 1, % Output Flag: 1=On, 0=Off 
Flags2(10)= 1; % Traee Flag: 1=On, O=OIT 
Flags2(11)= 0; % Symmetrie Geometry Flag: 1=Sym, 0=NonSym 
Flags2(12)= 0, % Inverse Kinematies Bypass: 1=Bypass, 0=Inverse \inematies 
Flags2(13)= 0; % Torg Flag: O=No Torg, 1=One Torg, 2=Two Symmetrie Torgs 
%  Torg Flag is for when the bypass 1s enabled 
Flags2(14)= 0; % TorgCap: 1=On, 0=Off 
Flags2(15)= 0.075;% Limit on maximum wheel torque 
Flags2(16)= 0; % DocFlag: 1=On, O=Ott 
% separate meta files for each page ("doc#.met") 
DiaFlag =0; % Diary Flag 
%  1= Create diary file "main dia" 
% O=No diary file 
ConstMat = ones(3,k+3); 
for n=1:k+3 
ConstMat(2,n) = k+6-n; 
ConstMat(3,n) = ConstMat(2,n)*(ConstMat(2,n)-1); 
end 
if OpiF lag 
{[UpCoef,options] = fminu2(‘MainMin’,UpCoef0,options,[],ConstMat,Flags1); 
end 


if DiaF lag 


diary main.dia 
end 
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if ~OptF lag 
Bpeoei— UpCoey: 
end 
[JInt] = Main2(UpCocf,ConstMat,I‘lags2), 


% Plot position, velocity, & aeceleration reference trajectones 
k = Iength(UpCoef), 
A543 = inv(ConstMat(:,k+1:k+3))*({1; 0, 0} - ConstMat(:,1:k)*UpCoef), 
Coef = [UpCoef, A543], % Referenee trajeetory polynomial eoeffieients 
k = length(Coef); 
etcps — 21; 
form = 1:Steps 
Tau = (m-1)/(Steps-1), 
for n=1:k 
CTau(k+1-n) = Coef(k+1-n)*Tau%(n+2), 
CTaud(k+1-n) = Coef(k+1-n)* Tau“%(n+1); 
CTaudd(k+]1 -n) = Coef(k+1-n)*Tau“(n); 
end 
W(m) = ConstMat(1,:)*CTau'; 
Wd(m) = ConstMat(2,:)*CTaud'; 
Wdd(m) = ConstMat(3,:)*C Taudd’, 
end 
elg 
T=0:1/(Steps-1):1; 
subplot(22 1) 
plot(T,W),title(Position vs Normalized Timc’), 
xlabel(‘Tau (sec)’);y label (‘Position’), 
subplot(222) 
title(Refcrence Trajectorics’) 
subplot(223) 
plot(T,Wd);title(’ Velocity vs Normalized Time’): 
xlabel('Tau (sec)’);y label (‘Velocity’), 
subplot(224) 
plot(T,Wdd).ttle(Accelcration vs Normalized Timc’), 
xlabel(‘Tau (sec)’);ylabel('Acceleration’), 
if Flags2(1) 
meta main 
end 
if Flags2(16) 
meta doe6 
end 
pause 


disp(‘Initial guess for highest order eoefficients’).disp(U pCoef0’). 
disp(‘Cocfficients in descending order');disp(Coef); 
disp(‘Integral of Cost Function,(JIntAbs & HntSqr)'),disp(JInt), 
if OptFlag 

disp(‘Iterations');disp(options(10)); 
end 


diary off 


. Main2 


% Filename 1s "Main2.m" 

% This routine 1s the driver for the dual arm spacecraft problem 

% but is called by "MainOpt.m" after the polynomial reference trajectory 
% coefficients have been optimized. 
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function [JIntTotal] = Main2(UpCocf,ConstMat,Flags) 


% Calculate the coefficients for orders five, four. and three. 

% Include these with the higher order coefficients 1n a veetor. 

k = length(UpCoef); 

A543 = inv(ConstMat(:,k+1:k+3))*({1; 0; 0} - ConstMat(:,1:k)*UpCocf*): 
Coef = [UpCoef’; A543]; % Reference trajectory polynomial coefficients 


MANMVAW%% 

%% Times %% 

WAWMWVAV%% 

% Referenee Maneuver Start and Stop Times and Controller Stop Times 
% Setting the controller time longer than the refcrence maneuver time 
% ensures that the controller eliminates any errors remaining after the 
% reference trajectory should be complete. To exereise the eontroller 
% only with no refcrence trajectory, set the referenee maneuver stop 

% time to a negative valuc. 


TO = 0; 
TfR = 10; 
TIC = 10; 


MetaF lag = Flags(1); 
ContFlag = Flags(2); 
PertFlag = Flags(3); 
AMatFlag = Flags(4); 
Wheelk lag = Flags(5), 
FOMFlag = Flags(6); 
PinvFlag = Flags(7); 
KEFlag = Flags(8); 
OutFlag = Flags(9); 
Trace = Flags(10); 
SvmFlag = Flags(11); 
ByPass = Flags(12); 
TorgFlag = Flags(13); 
TorqCap = Flags(14); 
TorqMax = Flags(15); 
DocFlag = Flags(16); 


Pert =-10; % Perturbation of initial payload angle, ThcetaP (deg) 
Tol = le-6, % Integration tolcranee 

Interval = 3; % Stick figure drawing includes every Interval'th time 
R2D = 180/p1, % Conversion factor from radians to degrecs 


WHVWAVAVAMV%%%Y% 
%% System Parameters %% 
WWD V/V%VV%V%%Y%0% 
% Lengths (m) 


LO = 0.75, % Origin to left shoulder 
Li =0.5; % Left upper arm 
L2=0:5. % Left forcarm 
LP=O>: % Payload 

R2=0.5, % Right forearm 
R1=0.5; % Right upper arm 


RO = sqrt(2*0.75%2), % Origin to nght shoulder 
Ls = (bG. Vici 2 sera? tol anol, 


% Member masses (kg) 
MO =S; 

ML1 =]; 

ML2 = 1; 

MRI = 1; 
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MR2 = I; 
MP =1; 
Ms = [MO; ML1,; ML2; MRI; MR2; MP]; 


% Center of mass distances (m) 

LcO =0; 

Lel. | = 0.25, 

Pew? —(0. 25. 

Pelkoli— 0.25. 

LcR2 = 0.25, 

Pel —(.25: 

fie — 1c); Let; LeL2; LeR1; Lel&2; LeP]; 


% MOI about individual centcrs of mass 

% Arms arc modelled as slender rods: Ie = (1/12)*(mass)*(length)*2 
IO = MO, 

IL) =(1/12) * ML1 * L1%2,; 

eee — (1/12) * ML2 * L2%2: 

eae 4 1/12) * MRI * R1%2; 

Nee = (1/12) * MR2 * R22. 

ee lvl 2) * MP * LP” 2: 

fe— {10 IL); 1L2; IRI; IR2; IP]; 


% Nominal initial and desircd final locations of payload 
% Point Q is at wrist of left arm 

% Point P is at wrist of nght arm 

Som= 0.125; Qy0n= 1.5; 

PxOn = 0.625; PyOn = 1.5; 

Oxf =0.125;, Qyf = 1.0; 

Pxf =0.125; Pyf = 1.5, 


% Nominal initial and desired final locations of centcrbody 
ThOO = O/R2D; 
ThOf = O/R2D; 


% Arms mount locations wrt centerbody coordinate frame (rad) 
Thl.0 = pi/2, 

ThRO = pi/4; 

AngConst(1) = ThLO; 

AngConst(2) = ThRO; 


% Symmetnc geometry to center arms and test kinctic energy 
if SymF lag 

ThLO = 3*pi/4; 

AngConst(1) = ThL9O; 

RO=LO, % Ongin to nght shoulder 

Ls(7,1) = RO, 

Qx0n = -0.25; QyOn = 1.2; 

PxOn = 0.25; PyOn = 1.2; 

end 


% Asscmble information required in other subroutines into a matrix 


BoundC(1,1) = QxOn; BoundC(1,2) = QyOn; 
BoundC (2,1) = PxOn; BoundC(2,2) = PyOn; 
BoundC (3,1) = Oxf; BoundC(3,2) = Qyf, 
BoundC(4,1) = Pxf; BoundC (4,2) = Pyf, 
BoundC(5,1) = ThLO,; BoundC(5,2) = Tho: 
BoundC (6,1) = TO; BoundC(6,2) = TR; 
BoundC(7,1) = TfC; BoundC(7,2) = Contl lag: 
BoundC(8,1) = AMatFlag, BoundC(8,2) = Whecll lag; 
BoundC(9,1) = Thoo; BoundC(9,2) = ThoOf; 
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BoundC(10,1)= EOMF lag; BoundC(10,2)= PInvilag; 
BoundC(11,1)=KEFlag, BoundC(11,2)= ByPass: 
BoundC(1 2,1 )= TorgF lag; 

BoundC(13,1)= TorqCap; BoundC(13,2)= TorqMax; 


% Gip are gains for angle 1 position error 
% Giv are gains for angle 1 veloeity error 
Gpos = 0.5; % Position error gain 

Gvel =0.2, % Velocity error gain 
Gains = [Gpos; Gvel], 


% Weighting Matriees 

% Control torques are ealeulated to minimize the following cost function: 
% J=0.5*(u'*Wu*u + (A'™*Lam)'*We*(A'*Lam)) 

% Wuis the eontrol torque weighting matrix 

% Weis the constraint force weighting matrix 


if WheelF lag 
Wu = eye(7); % Control Torque Weighting 
else 
Wu = eye(6); 
end 
Yif WheelF lag 
% Wu = zeros(7,7); 
“else 
% Wu = zeros(6,6); 
%end 


%Wu(4,4)=1e5, % Penalty on wrist motors for free centerbody ease 
%Wu(7,7)=1e5; 

%Wu(2,2)=lel0. % Penalty on wnst motors for fixed eenterbody ease 
%Wu(5,5)=lel0; 

We = zeros(8,8); % Constraint Force Weighting 

“We = eye(8); 


MWAMANVMMMMAMANMVM%M%% 
%% INITIAL CONDITIONS %% 
MAMMA MVVMMMMM%M%Y%O% 

% Desired Initial Payload Parameters 
ThPO = atan2(PyOn-QyOn,PxOn-QxOn); 
XcQ) = 0.5 * (PxOn + QxOn), 

YeO = 0.5 * (PyOn + QyOn): 


if PertFlag % Perturbation enabled 
ThPO = ThPO + Pert/R2D, % Perturb payload angle 
Qx0 = XeO - LeP*eos(ThPO); % Perturb arm end points 
Qy0 = YeO - LeP*sin(ThP0); 
PxO = Xe0 + (LP-LeP)*eos(ThPO),; 
PyO = YeO + (LP-LeP)*sin(ThPO); 


else % No Perturbation 
QOxnO = QxOn; 
Qy0 = QyOn; 
PxO = PxOn; 
PyO = PyOn, 
end 


PertCrd = [Qx0 Qy0 PxO Py]: 


% Left Arm 

% Elbow 1s left of line from arm base to Q (RQ) 

LSx = LO * eos(ThOO + ThLO); 

LSy =LO * sin(Th0O + ThLO), 

RQ = sqrt((Qx0-LSx)*2+(Qy0-LSy )*2); % Length from arm base to Q 
Betal = atan2(Qy0-LSy,Qx0-LSx),; % Angle from arm base to RQ 
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volwaw OL.cosines: cos(A) = (b*2 + c*2 =a"2)/(2he) 

% Apply to find angle between RQ and Link [.1 

Nani 1*2 + RO? ~12%2. 

Den =2 * L1 * RQ; 

Beta2 = acos(Num/Den), % Angie from RQ to Link | 
ThI. 10 = (Betal + Beta2) - (ThOO + ThLO), % Theta L1 
% Usc law of cosines to find the interior angle at the elbow 
Nene 1,1°2 4+12%2 - RQ*2: 

en — 2 * 1,1 * L2: 

Beta3 = acos(Num/Den); 

ThL 20 = -(pi-Beta3), 


% Right Arm 

% Elbow is nght of line from arm base (shoulder) to P (wrist) (RP) 
RSx = RO * cos(Th0O + ThRO); 

RSy = RO * sin(Th0O + ThRO); 

RP = sqrt((PxO-RSx)42+(Py0-RSy)*%2); % Length from arm basc to P 
Betal = atan2(Py0-RSy,Px0-RSx); % Angle from arm base to RP 
% Law of cosines: cos(A) = (b%2 + c%2 - a®2)/(2bc) 

% Apply to find angle between RP and Link R1 

Num = R142 + RP%2 - R2%2; 

Pen 2 * Rl] * RP: 

Beta2 = acos(Num/Den), % Angle from Link R1 to RP 
Beta4 = Betal - (ThOO + ThRO), 

ThR10 = -(Beta2 - Beta4), 

Num = R1“2 + R22 - RP%2; 

Dene 2* Ri * 2: 

Bcta3 = acos(Num/Den),; 

ThR20 = pi - Beta3; 


% Desired Initial Statc 
XO = [ThOO;, ThL 10; ThL20; ThR10; ThR20; ThPO, XcO; YeO:.. 
oe 0. 0 0; 0; 0; 0: OF; 


MPWAVMAMMV%%0%%M%%% 
%% Kinetic Energy Test Conditions %% 
MAWMAWMMDVMVVVMMMMMMMM%% 
% Specify Payload and Centerbody Initial Rates 
% Compatible Ratcs for the Redundant Coordinates arc calculated 
if KEFlag 
ThPd0 = 0/R2D;. % Rates to specify 
Xcd0 = -0.03; 
Ycd0O = -0.03; 
ThOd0 = O/R2D, 
MAVMV%VV%%%% 
%% LEFT ARM %% 
MMMM %%% 
% |Qxd. Qyd] = [H1]*ThOd + [H2]*Thd 
% QOxd & Qyd are x & y components of point Q inertial velocity. 
% Thd = [ThL Idot, ThL 2dot] 
% H matrices are made from expressing the x & y components of Qin 
% terms of LO, ThO, ThLO, L1, ThL1, L2, and ThL2. 


% Qx=L0*cos(ThO+ThLO)+L | *cos(ThO+ThLO+TL 1)+L2*cos(Tho-+... 
% HieOF he lsh 2) 

% Qy=L0*sin(ThO+ThLO)+L 1 *sin(ThO+Thi_O+ThL1)+b2*sin(Tho-+... 
% THEGT IE een 2} 


% The diffcrentiation of these equations lead to 

% {Qxd; Qyd] = [H1]*ThOd + [H2]*Thd which can be solved for Thd 
Qxd0O = Xcd0 + LcP * ThPdO * sin(ThPO); 

Qyd0 = YcdO - LeP * ThPdO * cos(ThPO); 

H2(1,2) = -L2*sin(Th0O0+ThLO+ThL 10+ThL20), 
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H2(1,1) = H2(1,2) - L1*sin(Th0OO+ThLO+ThI. 10): 
112(2,2) = 1.2*eos(ThOO+ThLO+ThL 10+ThL 20); 
112(2,1) = H2(2,2) + L1 *cos(ThOO+ThLO+ThL 10): 
111(1,1) = H2(1,1) - LO*sin(ThOO+ThLO), 
111(2,1) = H2(2,1) + LO*eos(ThOO+ThLO); 

ThdO = inv(H2) * ({Qxd0; QydO] - H1*ThOd0); 

% Angle rates 

ThL 1d0 = Thdo(1); 

ThL2d0 = Thd0(2); 

WWAVAVMAM%%% 

%% RIGHT ARM %% 

MWUAWVVVWVV%%%% 

“% The development is similar to the left arm 


% Px=RO*cos(ThO+ThRO)+R 1 *eos(ThO+ThRO+ThR | )+R2*cos(Tho-+... 
% ThRO+ThR1I+ThR2) 

% Py=RO*sin(ThO+ThRO)+R 1 *sin(ThO+ ThRO+ThR 1)4+R2*sin(Th0-+... 
% ThRO+ThR1+ThR2) 


% [Pxd; Pyd] = [H1]*ThOd + [H2]*Thd 
PxdO = XcdO - (LP - LeP) * ThPdO * sin(ThPO), 
PydO = Ycd0 + (LP - LeP) * ThPd0O * cos(ThPO); 
112(1,2) = -R2*sin(Th0O0O+ThRO+ThR10+ThR 20), 
112(1,1) = H2(1,2) - RI *sin(ThOO+ThRO+ThR 10); 
H2(2,2) = R2*eos(Th0O0+ThRO+ThR10+ThR20): 
H2(2,1) = H2(2,2) + RI *eos(ThOO+ThRO+ThR 10), 
H1(1,1) = H2(1,1) - RO*sin(Th0O+ThRO), 
H1(2,1) = H2(2,1) + RO*cos(ThOO+ThRO), 
ThdO = inv(H2) * ({Pxd0; PydO] - HI *ThOd0): 
% Angle rates 
ThR1d0 = Thd0O(1); 
ThR2d0 = Thd0O(2); 
XO = [ThOO, ThL10; ThL20,; ThR1I0; ThR20, ThPO: XcO: Yc0.... 
ThOd0; ThL1d0; ThL2d0; ThR1d0; ThR2d0; ThPd0; Xed0; YcdO}; 
end 


MWAVWVAVMAVVVMVMVNM%VV%N% 
%% FINAL CONDITIONS %% 
MANA RA NASARASANA SANA TA NASA RANA 
% Desired Final Payload Angle 
ThPf = atan2(Pyf-Qyf,Pxf-Qxf); 


% Left Arm 

% Elbow 1s left of line from arm base to Q (RQ) 

LSx = LO * eos(ThOf + ThLO), 

LSv =LO * sin(ThOf + ThLO); 

RQ = sqrt((Qxf-LSx)*2+(Qyf-LSy)%2); % length from arm base to Q 
Betal = atan2(Qyf-LSy,Qxf-LSx), % Angle from arm base to RQ 
% Law of eosines: eos(A) = (b%2 + c%2 - a®2)/(2bc) 

% Apply to find angle between RQ and Link L 1 

Num = L142 + RQ%2 - L2%2; 

Den =2*L1 *RQ; 

Beta2 = acos(Num/Den), % Angle from RQ to Link | 
ThL1f = (Betal + Beta2) - (ThOf+ThLO); % Theta 1.1 

% Use law of cosines to find the interior angle at the elbow 

Num = b1°2 #272 = RO. 

Den=2*~ Ei be. 

Beta3 = aeos(Num/Den),; 

ThL 2f = -(pi-Beta3); 


% Right Arm 


% Tlbow is right of line from arm base (shoulder) to P (wrist) (RP) 
RSx = RO * eos(ThOf + ThRO), 
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RSy = RO * sin(ThOf + ThR0), 

RP = sqrt((Pxf-RSx)%2+(Pyf-RSy)%2); % Length from arm base to P 
Betal = atan2(Pyf-RSy,Pxf-RSx); % Angle from arm base to RP 
% Law of cosines: eos(A) = (b%2 + e4%2 - a%2)/(2bc) 

% Apply to find angle between RP and Link R1 

Nun = RI“2 + RP - R2%2; 

Den — 2 * |x] * RP; 

Beta2 = acos(Num/Den),; % Angle from Link R1 to RP 
Beta4 = Betal - (ThOf + ThRO0); 

ThRIf = -(Beta2 - Beta4),; 

mum = RNA2 + R2°2 - RP*2- 

Pem— 2 *R] * R2; 

Beta3 = aeos(Num/Den), 

ThR2f = pi - Beta3; 


% Desired Final State 

Per 0,9 * (Pxf + Oxf); 

Yet =0.5 * (Pyf + Qy1); 

Q{Des = [ThOf; ThL If; ThL2f; ThRIf; ThR2f; ThPf. Xef; Yef:... 
OQ; O 0; 0; OE O07 20): 


if OutF lag 

MMMM M%M%%%MMMMMM%N% 

%% PROBLEM SUMMARY %% 
MMIMMMMVM%MMMMMM%M%% 

disp?PROBLEM SUMMARY’) 

disp(") 

disp(‘Initial Angles (deg)’) 

disp(‘Initial Angular Rates (deg/see)') 

disp( Desired Final Angles (deg)’) 

disp Theta0 ThetaL!] ThetaL2 ThetaRI Thetak2 ThetaP’) 
disp(X0(1:6)'*R2D) 

disp(X0(9:14)'*R2D) 

disp(Q{Des(1:6)'*R2D) 

disp(") 

disp(‘Payload Coordinates (m)') 

disp(’ Nominal Initial, Perturbed Initial, and Final’) 

disp6 — Qx Qy Bx Py’) 

TableCrd = [QxOn QyOn PxOn PyOn; PertCrd: Qxf Qvf Pxf Pyf]; 
disp(TableCrd) 

disp(") 

disp(Arm Mounting Locations wrt Centerbody Coordinate Frame (deg)') 
disp(ThLO*R2D);disp(ThRO*R2D) 

disp(") 

disp(‘Start, Reference Manuever Stop, & Simulation Stop Times (sec)’) 
disp(TO);disp(T£R);disp(TfC) 

disp(") 

disp('Controller Status (1 = On, 0 = Off)') 

disp(Contk lag) 

disp(") 

disp(‘Perturbation Status (1 = On; 0 = Off)') 

disp(PertF lag) 

disp(") 

disp(Centerbody Status in Forward EOM (1 = Fixed; 0 = Frec)’) 
disp(A MatF lag) 

disp(") 

disp(‘Reaetion Wheel Status (1 = On, 0 = Off)') 

disp( WheelF lag) 

disp(") 

disp Number of Equations in Cost Funetion Constraint (3, 5 or 8)') 
disp(EOMF lag) 


iy 


disp(") 
disp(‘Psuedo-Inverse Status (1 = On, 0 = Off)') 
disp(PInvF lag) 
disp(") 
disp(‘Nonzero Initial Velocity Status (1 = On. 0 = O/1)') 
disp(KEF lag) 
disp(") 
disp(‘'Geometry Status (1 = Symmetric; 0 = Nonsy mmetric)’) 
disp(SymF lag) 
disp(") 
disp(‘Inverse Kinematics Bypass Status (1 = Bypass: 0 = Usc inv. kinematics)’) 
disp(By Pass) 
disp(") 
disp(‘Torques 1f Bypass Enabled (O=None, !=Onc, 2=Two Symmetric)’) 
disp(TorqF lag) 
disp(") 
disp(‘Reaction Wheel Torque Cap Status (1=/nabled, 0=Disabled)') 
disp(TorqCap) 
if TorqCap 
disp(‘Limit on Wheel Torque’); 
disp(TorqMax); 
end 
disp(") 
disp(‘Controllcr Gains (position and velocity )') 
disp(’ Gpos — Gvel') 
disp(Gains’) 
disp(‘') 
disp(‘Cost Function: J = 0.5*(uT*Wu*u + (AT*Lam)T*We*(AT *Lam))') 
disp(’ where _ T signifies transpose’) 
disp(‘Control Torques Weighting Matrix, Wu’) 
disp( Wu) 
%disp('Constraint Forccs Weighting Matrix, Wc’) 
Yodisp( We) 


end % End of OutFlag branch 


MWWUMV%%V%%%%%%% 

%% INTEGRATION %% 

MWWWVVVVVV%%0%% 

% "ode" is a variable step size Runge-Kutta integrator function 

% supphed with MATLAB. "“ode2" is the same as "ode" in its function 

% but permits the passing of more variables into and out of the function. 

[T,X,Torg. TorqRef,A qdot,J,Res,LHS,RHS,Delq] = ... 
odce2(‘Eqn2', TO, T£C,X0, Tol, Trace,Ls,Ms,CMs., Is, BoundC.,... 
Gains, QfDes, Wu, Wc,Coef,ConstMat); 

k = length(T), 

JIint = XC.,17:18); 

JintTotal = X(k, 17:18); 


if OutFlag 

YVVV%V%%o%%%o0% 
%% OUTPUT %% 
YUVA V~VVVV%M% 


cle: 

% Angle Histories 

n = length(T), 

Q = X(:, 1:6); 

subplot(221) 

plot(T.OC) 1)*#R2D, Oc 2)*R2D 10G3) RZD 
TOG 4 2D TOG S)2R2D 1 OCG Ak Zb). 
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hold on 

Mohn) Ofpes( ji k2D,* 1 (ny Ofbes(2)*k2D/*" (a), OlDes(3)* 2D," ™.... 
ihn) Olmes4 FR 2D * (ny OlbDes(o)*1k2D. 4. Min) OtDes(a)* ik 21), *); 

nileC Thetas vs line). 

Nlabel('Time (see)’);ylabel(Angles (deg)’); 

hold off 

% Angle Rate Histories 

Qdot = X(:,9:14); 

subplot(223) 

mow 1 OdotC, |)*R2D,T,Qdot(:,.2)*R2D, T,Qdot(.,3)* R20)... 
MeOdou 4)*R2D 1 Odotl,5)*K2D, TV Odot( Gy KZ): 

title("ThetaDots vs Time’); 

xlabel('Time (see)');ylabel(Angle Rates (deg/see)’); 


% Departures from Reference Trajectory 
if ~ByPass 
subplot(222) 
plot(T,Delq(1,:)*R2D,T,Delq(,:)*R2D,T,DelqG,:)*R2D.,... 
elat4 7 k2D,) Dela, 7 R20. I, Dela," K2D): 
title(Displaeement Errors vs Time’), 
xlabel("Time (see)’):ylabel((Q-QRef (deg)’): 


end 


if MetaF lag 
meta main 

end 

if DocFlag 
meta doc] 

end 

pause 


% Draw Motion 
Angles = Q¢:,1:6). 
| Xeoord, Ycoord] = Draw3(Ls,AngConst,Angles, Interval); 
if MetaF lag 
meta main 
end 
if DoeF lag 
meta doe2 
end 
pause 


disp("), 

disp(STATE VECTOR TIME HISTORY:’); 

disp(‘Angles (deg)’) 

Table! = [T XC, 1:6)*R2D}]. 

disp = Time’ =—Theta0 ThetaL! ThetaL2 ThetaRkI] Thetak2 ThetaP’), 
disp(Tablel) 

pause 


disp("); 

disp(Angle Rates (deg/sec)’) 

Table2 = [T Qdot(:,1:6)*R2D}]; 

disp? Time ThOdot ThLidot ThL2dot ThRiIdot ThR2dot = ThPdot’); 
disp(Table2) 

pause 


if ~ByPass 
disp("), 
disp(TRAJECTORY ERROR TIME HISTORY:’).: 
disp( Angles (deg)’) 
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Table2a = [T R2D*Delq(1:6,:)']; 
disp’ Time’ =DelThO DelThL1 DelThL2 DelThRI DelThR2 DelThP'); 
disp(Table2a) 

end 

pause 


% Referenee Torque Histories 
elg: 
if TIR>O 
if THiR= Pte 
[r,s] = size(TorgRef), 
TorqRef = [TorqRef zeros(s,1)]; 
TRef = [TC1:r); TAR]; 
else 
TRef = T, 
end 
subplot(221) 
plot(TRef, TorqRef); 
title(Referenee Torques vs Time’); 
xlabel(‘Time (see)');ylabel (Reference Torques'), 
end 
% Command Torque Histories 
% Torg = {Torq, zeros(4,1)]; 
k=n, 
subplot(223) 
plot(T(1:k)', Torq); 
title((Command Torques vs Time’); 
xlabel(‘Time (sec)');ylabel(Command Torques'’), 


% Cost Funetion 
subplot(222) 
plot(T,J(1,:)),title(Cost vs Time'), 
xlabel(‘Time (see)'),ylabel(‘J=abs(Uwh)’), 
subplot(224) 
plot(T,Jint);title(Integrated Cost vs Time’), 
xlabel("Time (see)');ylabel(JInt’); 
if MetaF lag 
meta main 
end 
if DoeF lag 
meta doe3 
end 
pause 


if TR > O 
disp(") 
dispCREFERENCE TORQUE HISTORY’), 
if WheelF lag 
disp( Time UO ULS ULE ULW URS URE URW’), 
else 
disp( Time ULS ULE ULW URS URE URW’), 
end 


Table4 = [TRef TorqrRef]; 


disp(Table4) 

end 

pause 

disp(") 

disp(COMMAND TORQUE HISTORY’), 

if WheelFlag 

disp" Time UO ULS ULEVULEW URS UKE URW: 
else 
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disp( Time ULS ULE ULW URS URI: URW’), 
end 
TableS = [T(1:k) Torq’), 
disp(Table5) 


pause 


Tableo = [TC1:k) Jd.,:)' Int]; 

disp"); 

disp(COST FUNCTION HISTORY’); 
disp(’ Time J JintAbs  JintSqr'); 
disp(Table6),; 

pause 


% Angular Momentum 
k = length(T); 


forn= 1:k 
[Ils] = AngMo(Ls,Ms,CMs,Is,X(n,1:8),X(n,9:16)); 
ifn == 
HHist = Hs; 
else 
HHist = {HHist; Hs]; 
end 
end 
clg 
subplot(221); 


plot(T,HHist(:,1:6));title(Angular Momentum of Pieces vs Time’): 
xlabel(‘Time (see)');ylabel( Ang Momentum (N-m-see)’), 
subplot(223); 
plot(T,HHist(:,7));title('Total Angular Momentum vs Time’). 
xlabel(’Time (sec)');ylabel( Ang Momentum (N-m-sec)’). 
% Kinetic Energy 
for m = 1:k 
if AMatFlag 
, [M,G,A,Adot,B] = MatxFix(Ls,Ms,CMs,Is,X(m,1:8),X(m,9:16),AngConst); 
else 
[M,G,A,Adot,B] = Matx(Ls,Ms,CMs,Is,x(m,1:8),(m_,9:16),AngConst): 
end 
LIISTot(m) = 0; 
RIISTot(m) = 0; 
ResTot(m) = 0, 
for n=1:8 
LHSTot(m) = LHSTot(m) + LHS(n,m); 
RHSTot(m) = RHSTot(m) + RHS(n,m), 
end 
ResTot(m) = LHSTot(m) - RHSTot(m); 
KE(m) = 0.5*X(m,9:16)*M*X(m,9:16)'; 
end 
subplot(224) 
plot(T,KE);title(Kinetic Energy vs Time’); 
xlabel(’Time (sec)');ylabel(KE (kg m‘%2/s%2)'). 
% Compare wheel torque to change in total angular momentum 
Id = J(2,:): 
subplot(222) 
plot(T(1:k)', Torg(],:), TCI :k)',Hd); 
titleCompare Wheel Torque to Change in Ang. Mom.’), 
xlabel(’Time (sec)'); 
pausc 
if MetaF lag 
meta main 
end 
if Dock lag 


14] 


meta doe4 
end 
“a pausc 


clg 
subplot(221) 
plot(T,Res).title(Residuals of Equations’), 
xlabel(‘Time (sec)’);ylabel( LHS-RHS’); 
subplot(223) 
plot(T,ResTot):title(Total Residuals’); 
xlabel(‘Time (sec)');ylabelCLHS-RHS') 
%, Constraints: see if A*Qdot = 0 is satisfied 
subplot(222) 
plot(T(1:k),A gdot(1,:), Td :k),Aqdot(2,:), TC :k). Aqdot(3,:).... 
T(1:k),Aqdot(4,:)); 
[dum 1 ,dum2] = size(Aqdot); 
if dum] ==5 
hold on 
plot(T(1:k),Aqdot(S,:)); 
hold off 
end 
title(Constraints: A*Qdot vs Time’); 
xlabel('Time (sec)');ylabel(‘A *Qdot'); 
if MctaF lag 
meta main 
end 
if DoeF lag 
meta doc5 
cnd 
pause 


end % End of OutFlag braneh 


NMiatx 


% Filename 1s 'Matx.m' 

% This routine calculates the matrices for the dual arm 

% spacecraft EOM when it 1s grasping a payload. Each arm 
% has two links. This version assumes that the centerbody 
% 1S NOT fixed. This impacts A and Adot. 


function [M,G,A,Adot,B] = Matx(Ls,Ms,CMs.,]s,Ths, Thdots,AngConst) 


% OUTPUTS: 

% M = 8x8 mass matrix 

% G = 8x1 vector with conolis and centnpctal terms 
% A = 4x8 constraints matrix 

% Adot = 4x8 derivative of constraints matrix 

% B = Control influenee matrix 

% 

% INPUTS: 

% Ls = 7x1 vector of lengths (m) 


% Ist elcment = distance from origin to left arm mount 

% 2nd & 3rd elements wrt left arm (from shoulder to wrist) 
% 4th element = payload length 

% Sth & 6th elements wrt right arm (from wrist to shoulder) 
% 7th element = distanee from right arm mount to ongin 


% [EO Ea RoR RO 
% Ms = 6x1 eolumn veetor containing the masses (kg) 


142 


% Ist clement = mass of spacecraft centerbodv 


% 2nd & 3rd elements = left arm (upper then lower arm) 
%) 4th & Sth elements = right arm (upper then lower arm) 
% 6th element = payload mass 


% [MO; ML]; ML2,; MR1; MR2; MP] 


% CMs = 6x1 column vector containing center of mass locations (m) 


% HecO Werlich?) UckKl. Wek2- el?) 

% 1s = 6x] column vector containing the moments of inertias 
% about the respective body's center of mass (kg m“%2) 

% Ist clement = inertia of spacecraft centerbody 

% 2nd & 3rd clements = left arm (upper then lower arm) 
% 4th & Sth elements = night arm (upper then lower arm) 
% 6th element = payload inertia 


% HO SHE) Me2 RA 2, LP) 

% Ths = 6 element vector containing the angles which deseribe 
% the configuration of the system. 

% Pho) thir. (hei TR) The] 

% Thdots = 6 element vector containing the angle rates 


% AngConst = 2 element vector of arm mounting locations 
% [ThLO; ThRO] 


MMMWVVMAMMMMMMVM%M%MMMMMMM MM MMV %% 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
MWVWVAM%MW%M%%AMMMMMMMMMMMM% MMMM MYY% 
% Lengths (m) 

LO = Ls(1); 

1.1 = Ls(2); 

1.2 =Ls(3), 

Pee es(4): 

R2 = Ls(5); 

RI = Ls(6); 

RO =Ls(7); 


% Member masses (kg) 
MO = Ms(1); 

ML 1 = Ms(2); 

ML2 = Ms(3); 

MR1 = Ms(4), 

MR2 = Ms(5); 

MP = Ms(6), 


% Center of mass distances (m) 

LcO =CMs(1); 

LeL1] =CMs(2); 

relbz = CMs(3); 

Lek] = CMs(4); 

LeR2 = CMs(5),; 

IL.cP =CMs(6), Yomeasurcd from left end 


% MOI about center of mass 


IO = Is(1): 
Il.1 =Is(2); 
11.2 =Is(3); 
IR] = 1s(4), 
IR2 = 1s(S), 
IP = Is(6); 
Y%) Angles 


ThO = Ths(1); 
ThL1 = Ths(2): 
ThL2 = Ths(3); 
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ThR1| = Ths(4); 
ThR2 = Ths(5); 
ThP = Ths(6); 


% Angle Ratcs 
ThOd = Thdots(1); 
Th ld = Thdots(2),; 
ThL2d = Thdots(3); 
ThRid = Thdots(4); 
ThR2d = Thdots(5),; 
ThPd = Thdots(6); 


% Arm mount locations 
ThLO = AngConst(1); 
ThRO = AngConst(2); 


MMMM MM%% 

%% Mass Matnx %% 

WLLL WANA NL AWA NALA 

M = zeros(8,8); 

M(&,8) = MP; 

M(7,7) = MP; 

M(6,6) = IP; 

M(5,5) = IR2 + MR2*LcR2%2; 

M(5,4) = M(5,5) + MR2*R1*LcR2*cos(ThR 2), 

M(4,5) = M(5,4); 

M(5,1) = M(4,5) + MR2*RO*LcR2*cos(ThR1+ThR2); 

M(1,5) = M(5,1); 

M(4,4) = M(4,5)+IRI+MR2*R1*LcR2*cos(ThR2)+MR1 *LeR1I42+MR2*R 1%2, 

M(4,1)=M(4,4)+RO*(MR1*LcR1I+MR2*R1)*cos(ThR1)+MR2*RO*FLCR2*... 
cos(ThR1+ThR2); 

M(1,4) = M(4,1); 

M(3,3) = IL2 + ML2*LceL2%2; 

M(3,2) = M(3,3) + ML2*Li*LcL2*cos(ThL 2); 

M(2,3) = M(3,2); 

M(3,1) = M(3,2) + ML2*LO*LcL2*cos(Thh 14+Thh 2); 

M(1,3) = M(3,1); 

M(@.2) =MG.2)4ME2* LI*Lel2*cos( Thil2).1 ll -Miib ell eile 2 lee 

M(,I)=M(C@,2)-07 (vib Il *Lel I4+ML2*L1)*cos Thiam Oeil 
cos(ThL1+ThL2); 

M(1,2) = M(2,1); 

Part] = JO+M(2,2)+M(4,4)+M0*Lc0%2+(ML 1+ML2)*L.0%2+(MR 1+MR2)*RO*%2; 

Pait2 = 2*LOFUVIET* Cell aMiL2* il) * cos hile 27 Mile COA nCh Zoe 
cos( [nile + bnle 2); 

Part3 = 2*RO*(MR1*LCRI+MR2*R1)*cos(ThR 1)+2*MR2*ROFLCR2*... 
cos(ThR1+ThR2)-; 

M(1,1) = Part] + Part2 + Part3.: 


MWWW%%Y% 

w% GNM% 

WwWWVYY% 

G = zcros(8, 1); 

Pt] = -LO*(ThL 1d42+2* Th0d* ThL 1 d)*(ML1*LcL 1+ML2*L1)*sin(Th 1); 

Pt2 = -ML2*L1*LcL2*ThL2d*(2*ThOd+2*ThL 1d+ThL2d)*sin(Thl. 2); 

Pt3=-ML2*LO*LcL 2*(2* Th0d*(ThL 1d+ThL2d)+(ThL 1d+ThL2d)*%2)*... 
sin(Thl. 1+ThL 2); 

Pt4 = -RO*(ThR 1d*%2+2*ThOd* ThR 1d)*(MR 1 *LeR1I+MR2*R1)*sin(ThR 1); 

PtS = -MR2*R1*LcR2*ThR2d*(2* ThOd+2* ThR 1 d+ThR2d)*sin(ThR 2). 

Pt6=-MR2*RO*LcR2*(2* ThOd *(ThR 1d+ThR2d)+(ThR1d+ThR2d)*%2)*... 
sin(ThR1+ThR2); 

Gl) =Ptl +Pi2 4 Fits + Pa st Pia Fic: 


144 


Pi — 0 = hod" 2*(MLI*Lel ia Mi2*11)*sint( Thi): 

Mie = Mee lee 2* Th 2d*2* 1 h0d+2* Philids Thi2d)*sin(lhi.2). 

Pes = Mi2*10*Lc).2* 7hod*2*sin(ThLl1+Thi.2), 

G2) = Pil + Pt2 + P13: 

3) = MiE2* (e271 *(yhOd+ lh 1d)*2 *sinC) hl.2)41.0*7 Thod*2* 
sin(Thl.14+ThL2)); 

Pt) = RO*ThOd%2*(MR 1 *LeR 1AMR2*21)*sin( Thi 1); 

Pt2 = -MR2*R1*LeR2* ThR2d*(2* ThOd+2* Th ld+?hR2d)*sm(Thhk2), 

Pt3 = MR2*RO*LCR2* ThOd*2*sin(ThR 1+ThhR2); 

G4) = Ptl + Pi2 + Pts; 

oy Mik2* cit 2* (ik 1* Cl hOd+T hilt 1d)*2* sin hik2)+RO* Phod*2* 
sin(ThR1+Thik2)), 


WWW WAMWMV%%%% 

%% Constraints Matrix %% 

MMIMVVWWWWVWV%%%%) 

% The constraint matrix comes from putting the constraint equations 
% into the Pfaffian form: A*qdot + AO = 0. The first two constraint 
% equations are found by finding the x and y components of the 

% Payload's center of mass by starting at the origin and moving 

% up the Ieft arm. The second two constraint equations find the x 

% and y components of the Payload's center of mass by starting at the 
% origin and moving to the basc of the right arm and then 

% up the nght arm. Differentiating these equations results 

% 1n the Pfaffian form with AQ = 0. 

A = zeros(4,8),; 


A(1,/)=-1; 
A(2,8) = -1; 
A(3,7) = -1, 
A(4,8) = -1; 


ee) = -LeP*sin( lhl’); 

A(2,6) = LeP*cos(ThP); 

AG3,6)= C.P-LeP)*sm(ThP), 

74.6) = -(LP-LeP)*cos(ThP); 

A(4,5) = R2*cos(ThO+ThRO+ThR I+ThhR2); 
A(4,4) = A(4,5) + RI *cos(ThO+ThRO+ThER 1); 
A(4,1) = A(4,4) + RO*cos(ThO+ThRO),: 
A(3,5) = -R2*sin(ThO+ThRO+ThR 1+ThR2), 
A(G,4) = AG,5) - R1*sin(Th0O+ThRO+ThR 1); 
A(3,1) = AQG,4) - RO*sin(ThO+ThRO); 

A(2,3) = L2*eos(ThO+ThLO+ThL I+ThL2); 
A(2,2) = A(2,3) + LI *cos(ThO+ThLO+ThL 1); 
A(2,1) = A(2,2) + LO*eos(ThO+ThLO),; 
A(1,3) = -L.2*sin(Th0O+ThLO+ThL }+Th 2); 
A(1,2) = AC1,3) - LI *sin(Th0+ThlLO+Th 1): 
A(I,1) = AC1,2) - LO*sin(ThO+ThLo),; 


Adot = zeros(4,8); 

Adot(1,6) = -ThPd*LcP*eos(ThP); 

Adot(2,6) = -ThPd*LeP*sin(ThP), 

Adot(3,6) = ThPd*(LP-LeP)*cos(ThP), 

Adot(4,6) = ThPd*(LP-LeP)*sin(ThP), 

Adot(4,5) = -(ThOd+ThhR I}d+ThR2d)*R2*sin(ThO+ThRO+THR 1+ThR2), 
Adot(4,4) = Adot(4,5) - (Th0d+ThR1d)*R 1*sin(ThO+ThRO+ThR 1); 
Adot(4,1) = Adot(4,4) - ThOd*RO*sin(ThO+ThRO),- 

Adot(3,5) = -(ThOd+ThR 1d+ThhR2d)*R2*eos(Th0+ThRO+ThRI+ThR2), 
Adot(3,4) = Adot(3,5) - (ThOd+ThhR1d)*R1 *¥cos(ThO+ThRO+ThER 1); 
Adot(3,1) = Adot(3,4) - Th0d* RO*cos(ThO+ThRO), 

Adot(2,3) = -(ThOd+ThL 1d+ThL2d)*L2*sin(Th0+ThLO+ThL 14+ThL2); 
Adot(2,2) = Adot(2,3) - (ThOd+ThL 1d)*L I *sin(Th0O+ThLO+Th_L 1); 
Adot(2,1) = Adot(2,2) - ThOd*LO*sin(ThO+ThLO), 
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Adot(1,3) = -(ThOd+ThL 1d+ThL 2d)*1.2*cos(Th0+Thl.0+ThL 1+ThL 2); 
Adot(1,2) = Adot(1,3) - (ThOd+ThL 1d)*L | *cos(Th0+ThLO+ThL 1); 
Adot(1,1) = Adot(1,2) - Th0d*LO*cos(Th0+ThLQ); 


YMMV % 
MB YN 
MUW%%N% 
3 = zeros(8,6); 
BC1,3) = -1; 
B(1,6) = -1; 
B(2,1)= 1; 
B(2,3)= 
B(3,2) 
B(3,3) 
B(4,4) 
B(4,6) 
) 
) 
) 
) 


ey 

le 

| 

le 

=| : 
3(5,5 l 
13(5,6) = -1 
B(6,3 ] 
B(6,6 | 
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> 
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Matx Fix 


% Filename 1s 'MatxFix.m' 

% This routine calculates the matrices for the dual arm 

% spacecraft EOM when it is grasping a payload. Each arm 
% has two links. This version assumes that the ccnterbody 
% is fixed. This impacts A and Adot. 


function [M,G,A,Adot,B] = Matx(Ls,Ms,CMs, Is, Ths, Thdots.AngConst) 


% OUTPUTS: 

% M = &x8 mass matrix 

% G = 8x]I vector with conolis and centnpetal terms 
% A = 5x8 constraints matrix 

% Adot = 5x8 derivative of constraints matnx 

% B= Control influence matrix 


% 

% INPUTS: 

% Ls = 7x]! vector of lengths (m) 

% Ist element = distance from origin to Icft arm mount 

% 2nd & 3rd elements wrt left arm (from shoulder to wrist) 
% 4th element = payload Icngth 

% Sth & 6th elements wrt nght arm (from wrist to shoulder) 
% 7th element = distance from nght arm mount to ongin 


% WO. e2: LP R2 RO} 
% Ms = 6x1! column vector containing the masscs (kg) 


% Ist element = mass of spacecraft centcrbody 

% 2nd & 3rd elements = left arm (upper then lower arm) 
% 4th & Sth elements = mght arm (upper then lower arm) 
% 6th element = payload mass 


% [MO,; ML1; ML2; MRI; MR2; MP} 
“% CMs = 6x! column vector containing center of mass locations (m) 


% [EcO ck leUch 2. WeRI Wer? Cel] 

% 1s = 6x] column vector containing the moments of inertias 
% about the respective body's center of mass (kg m%2) 
% Ist element = inertia of spacecraft centerbody 

% 2nd & 3rd elements = Icft arm (upper then lower arm) 


% 4th & Sth elements = nght arm (upper then lower arm) 
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% 6th element = payload inertia 

% Omelet TRAST 2 SIP] 

% Ths = 6 element vector containing the angles which describe 
% the configuration of the system. 

% OS ieetne 2 ink Thk2. th) 

% Thdots = 6 element vector eontaining the angle rates 

% AngConst = 2 element vector of arm mounting locations 

% [ThLO: ThRO] 


MAMMMMMMMM%MMM%MMMMMMMMMMMMMMUUYN% 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
MPWWWMUMVMVVVVMUMMMMMMMMMANMMM%M 
% Lengths (m) 

LO = Ls(1); 

Li =Ls(2); 

L2 = Ls(3), 

LP = Ls(4); 

R2 =Ls(5); 

RI =Ls(6); 

RO = Ls(7),; 


% Member masses (kg) 


MO = Ms(1); 
MLI = Ms(2); 
ML?2 = Ms(3). 
MR] = Ms(4); 
MR2 = Ms(5); 
MP = Ms(6), 


% Center of mass distanees (m) 

LeO =CMs(1); 

LeLI = CMs(2), 

LeL2 = CMs(3); 

Lek I = CMs(4); 

LeR2 = CMs(5); 

LeP =CMs(6); Yomeasured from Icft end 


% MO] about eenter of mass 


lO = Is(1), 

ILI =I1s(2), 
IL2 = Is(3). 
IR] = Is(4),; 
IR2 = 1s(5), 
IP? =I[s(6); 
% Angles 


Tho" = Ths(1),; 
ThL 1 = Ths(2), 
ThL2 = Ths(3); 
ThR1 = Ths(4); 
ThR2 = Ths(5); 
ThP = Ths(6); 


% Angle Rates 
ThOd = Thdots(1); 
ThL Id = Thdots(2); 
Thi. 2d = Thdots(3): 
ThRId = Thdots(4); 
ThR2d = Thdots(5),; 
ThPd = Thdots(6); 
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% Arm mount locations 
ThLO = AngConst(1); 
ThRO = AngConst(2); 


MMMM %M%M%%%M% 

%% Mass Matnx %% 

MWMNMMAVMVMM% 

M = zeros(8,8); 

M(8,8) = MP; 

M(7,7) = MP; 

M(6,6) = IP; 

M(5,5) = IR2 + MR2*LcR2%2, 

M(5,4) = M(S5,5) + MR2*R1*LcR2*cos(ThR 2). 

M(4,5) = M(S,4); 

M(5,1) = M(4,5) + MR2*RO*LcR2*cos(ThR 1+ThR2),; 

M(1,5) = M(5,1); 

M(4,4) = M(4,5)4+IRI+MR2*R1*LcR2*cos(ThR2)+MR 1 *LcR1I42+MR2*R 142; 

M(4,1)=M(4,4)+RO*(MRI*LcR]+MR2*R1)*cos(ThRI+AMR2*RO*LCR2*... 
cos(ThR1+ThR2); 

M(1,4) = M(4,1); 

MG.3) =1b2 = ML2*lel272. 

M(3,2) = M(3,3) + ML2*L 1*LcL2*cos(ThL2); 

M(2,3) = M(3,2); 

M(G.1I)= MG.2)ee MIE2 FO" hele? *cos( Tali ine). 

M(1,3) = M(3,1); 

M(@,2) = MG.2)4ME2*L)*LeL2*ces(hlL2 tI MEI 2a vile lee: 

M(2,1 )=M(2,2)+L0O*(ML 1*LceL 1+ML2*L 1)*cos(ThL 1)+ML2*LO*LcL2*... 
cos(ThL1+ThL2); 

M(1,2) = M(2,1); 

Part] = I0O+M(2,2)+M(4,44#M0*Lc0%2+(ML 14ML2)*L0%24+(MR 1+MR2)*RO%2; 

Part2 = 2*LO* (ML icL 1 4+ME2*E1 )*cos(T nl lI) t2*ML2* 0" rcl2 
cos(ThL1+ThL2); 

Part3 = 2*RO*(MRI1 *LcRI+MR2*R1)*cos(ThR1)+2*MR2*RO*LCR2*... 
cos(ThR1+ThR2); 

M(1,1) = Part] + Part2 + Part3; 


M%%%%% 

%% G Vw% 

MM%%% 

G = zeros(8,1); 

Ptl = -LO*(ThL 1d*2+2*ThOd*ThL 1d)*(ML 1*LcL 14ML2*L 1)*sin(ThL I); 

Pt2 =-ME2*EI*leL2* Th 2d*(2* Th0da2* Vl ld eine 2d) * sin inte): 

Pt3=-MLE2*EO* ele 2*(27 ThOd*( The lid-A1 i 2d) Phnle ie ine ale 
sin(ThL1+ThL2),; 

Pt4 = -RO*(ThR1d®2+2* ThOd*ThR 1d)*(MR1*LcRI+MR2*R 1)*sin(ThR 1); 

PtS = -MR2*R1*LcoR2*ThR2d*(2* Th0d+2*ThR I[d+ThR2d)*sin(ThR2); 

Pt6=-MR2*RO*LcR2*(2* ThOd*(ThR 1 d+ThR2d)+(ThR 1 d+ThR2d)%2)*... 
sin(ThR1+ThR2); 

G1) = Pil + Pi2 + Pts + Pi4 + Pts BiG: 

Pt] = LO*ThOd’%2*(ML1*LcLI+ML2*L 1)*sin(Thh 1): 

Pt2 = -ML2*L1*LceL2*ThL 2d*(2* Th0d+2*ThL 1d+ThL2d)*sin(ThL 2). 

Pt3 = ME2*80*el2*  heds2*sin( thine, 

G(2) = Pt] + Pt2 + Pt3; 

G(3) = ML2*LcL2*(L1 *(Th0d+ThL1d)*2*sin(ThL2)+L0*Th0d’2*... 

sin(ThL1+ThL2)); 

Ptl = RO*ThOd*2*(MR 1 *LcR1I+MR2*R1)*sin(ThR 1): 

Pt2 = -MR2*R1 *LcR2* ThR2d*(2* Th0d+2* Th 1d+ThR2d)*sin(Vhk 2). 

Pt3 = MR2*RO*LcR2*ThOd“2* sin(ThR1+ThR2); 

G(4) = Ptl + Pt2 + Pt3: 

G(5) = MR2*LcR2*(R1 *(Th0d+ThR1 d)*2*sin(ThR2)+RO*ThOd’2*... 

sin(ThR1+ThR2)); 
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WWW %%%%% 

%% Constraints Matrix %% 

WAV VVWVVVVV%%% 

% The constraint matrix comes from putting the constraint equations 
% into the Pfaffian form: A*qdot + AO =0. The first two constraint 
% equations are found by finding the x and y components of the 

“% Payload's center of mass by starting at the origin and moving 

% up the Icft arm. The sccond two constraint equations find the x 

% and y components of the Payload's center of mass by starting at the 
% Origin and moving to the base of the nght arm and then 

% up the nght arm. Differentiating these equations results 

% 1n the Pfaffian form with AO = 0. 

A = zcros(5,8),; 


A(5,1) = 1. 
A(I,7) =-1,; 
A(2,8) = -1; 
AG3,/7)=-1, 
A(4,8) = -1; 


A(1,6) = -LcP*sin(ThP), 

WG) — LeP*cos(ThP): 

A(3,6) = (LP-LcP)*sin(ThP); 

A(4,6) = -(LP-LcP)*cos(ThP),; 

A(4,5) = R2*cos(ThO+ThRO+ThR1+ThR2); 
A(4,4) = A(4,5) + RI *cos(ThO+ThRO+ThR 1); 
A(4,1) = A(4,4) + RO*cos(ThO+ThRO); 
A(3,5) = -R2*sin(ThO+ThRO+ThR I+ThR2); 
A(3,4) = A(3,5) - R1 *sin(ThO+ThRO+ThR 1); 
A(3,1) = AQG3,4) - RO*sin(ThO+ThRO),; 

A(2,3) = L2*cos(ThO+ThLO+ThL 1+ThL2),. 
A(2,2) = A(2,3) + LI *cos(ThO+ThLO+ThL 1); 
A(2,1) = A(Q2,2) + LO*cos(ThO+ThLO), 
A(1,3) = -L2*sin(ThO+ThLO+ThL 1+ThL2),; 
A(1,2) = A(1,3) - LI *sin(ThO+ThLO+Th_L 1); 
A(t,1) = AC1,2) - LO*sin(Th0O+ThLO), 


Adot = zeros(5,8); 

Adot(1,6) = -ThPd*LcP*cos(ThP); 

Adot(2,6) = -ThPd*LceP*sin(ThP), 

Adot(3,6) = ThPd*(LP-LcP)*cos(ThP), 

Adot(4,6) = ThPd*(LP-LcP)*sin(ThP); 

Adot(4,5) = -(ThOd+ThR Id+ThR2d)* R2*sin(Th0+ ThRO+ThR1I+ThR2), 
Adot(4,4) = Adot(4,5) - (Th0d+ThRId)*RI*sin(ThO+ThRO+ThR 1); 
Adot(4,1) = Adot(4,4) - ThOd*RO*sin(ThO+ThRO). 

Adot(3,5) = -(ThOd+ThR 1d+ThR2d)*R2*cos(Th0+ThRO+ThR 1+ThR 2), 
Adot(3,4) = Adot(3,5) - (Th0d+ThR1d)*R1*cos(ThO+ThRO+ThR 1): 
Adot(3,1) = Adot(3,4) - ThOd*RO*cos(ThO+ThRO), 

Adot(2,3) = -(ThOd+ThL 1d+ThL2d)*L2*sin(Th0O+Thi0O+ThL 1+Th.2), 
Adot(2,2) = Adot(2,3) - (ThOd+ThL 1d)*L 1*sin(ThO+ThLO+ThL 1): 
Adot(2,1) = Adot(2,2) - ThOd*LO*sin(ThO+ThLO), 

Adot(1,3) = -(ThO0d+ThL1d+ThL2d)*L 2*cos(Th0O+ThL_0O+ThL 1+ThL2); 
Adot(],2) = Adot(1,3) - (ThO0d+ThL1d)*L I *cos(ThO+ThLO+ThL 1); 
Adot(1,1) = Adot(1,2) - Thod*LO*cos(ThO+ThLO); 


WY 
“% B %% 
MWANVMVVY 

B = zeros(8,6); 
ba. 3)=-1; 
B(1,6) = -1; 
Ip 1) = 1; 
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. Ref2 


% Filename is 'Ref2.m' 
% Reference Maneuver using cost function 
% This routine assumes that the spacecraft centerbody 1s held fixed. 


function [Torques,QRef, QdotRef,A qdot,J,C1,C2,C3] =... 
Ref2(Ls,Ms,CMs,]s,BoundC,T, Wu, We,Coef,ConstMat) 


% OUTPUTS: 
% Torques = 7x1 eolumn vector of torques that should be applied at 


% time T if the motion 1s to follow the reference trajcctory 

%) exaetly. The vector is arranged as [U0; ULS; ULE; ULW, URS, URE; URW] 
% which are the centerbody torque followed by the torques at the 

Y, shoulder, elbow, and wrist of the left arm and then the right arm 

% respectively. 


% QRef = 8x1 column veetor of referenee generalized eoordinates 
% QdotRef = 8x1 column vector of referenee generalized veloeities 
% Aqdot = 4x1 or 5x1 column vector (depends on status of AMatFlag) whieh 


% check to see if the constraint equation A* Qdot = 0 1s satisfied 
% J = scalar value of the reaction wheel torque absolute value. This 

% number will be integrated to find the value for the cost funetion. 
“% Lyapunov Controller matrices (reference trajectory values) 

% C] = 8x7 matrix 


% C2 = 8x4 or 8x5 (depends on status of AMatFlag) matrix 
% C3 = 8x] matnx 


% 

% INPUTS: 

% 1.s = 7x1 vector of lengths (m) 

% Ist element = distance from ongin to left arm mount 

% 2nd & 3rd elements wrt left arm (from shoulder toward wrist) 
% 4th elemcnt = payload length 

% Sth & 6th elements wit nght arm (from wrist toward shoulder) 
% 7th element = distance from right arm mount to origin 


% One? LPoRZ RA RG) 
% Ms = 6x] column vector containing the masses (kg) 


% Ist element = mass of spacecraft centerbody 

% 2nd & 3rd elements = mass of left arm (upper arm then lower arm) 
% 4th & Sth elements = mass of nght arm (upper arm then lower arm) 
% 6th element = payload mass 


% {MO; ML]; ML2; MR1; MR2; MP] 

% CMs = 6x1 column veetor eontaining center of mass locations 

% tkbcO: [cL Wel2: eR]: Eck 2 er 

% 1s = 6x1 column vector containing the moments of inertias about the 


% respective body's center of mass (kg m‘%2) 

% Ist element = inertia of spacecraft centerbody 

% 2nd & 3rd elements = inertia of Icft arm (upper arm then lower ann) 
% 4th & Sth elements = inertia of nght arm (upper arm then lower arm) 


% 6th element = payload inertia 


% Oe IR 2 ie 
% BoundC = boundry conditions for the problem. The first column 


% contains the initial x and y component of points Q & P 

% respectively, the x component of the right arm base, the 

% problem start time, and the simulation stop time. The seeond 
% column contains the x and y component of points Q & P 

% respectively, the x component of the right arm base, the 

% stop time for the ideal reference mancuver, and a flag to 

% activate or deactivate the controller. The origin for the 

% x and y components ts the base of the left arm 

% | =time 


% Wu = 6x6 or 7x7 control torque cost weighting matrix 
% We = 8x8 constraint cost weighting matrix 
% Coef = (n-2)x] column vector of referenee polynomial coefficients 


% beginning with order n coefficient 
% ConstMat = 3x(n-2) matrix of coefficients for referenee displacement 
%) (row 1), velocity (row 2), and acccleration (row 3) 


MWMMMAMM’MMMMMMMANMAMMMMMMMMMMMMMN 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
MMMM MMMAMMMMMMMMMMNMMNMMMMMN% 
% |engths (m) 

1.0 =Ls(1), 

LL} =Ls(2), 

1,2 =Ls(3); 

LP =Ls(4); 

R2 = Ls(5), 

RI =Ls(6); 

RO = Ls(7); 


% Member masses (kg) 
MO = Ms(1); 

ML1 = Ms(2), 

ML2 = Ms(3); 

MRI = Ms(4); 

MR2 = Ms(5),; 

MP = Ms(6); 


% Center of mass distanees (m) 

LeQ =CMs(1); 

].ecL] = CMs(2); 

LeL2 = CMs(3); 

].eRI = CMs(4); 

LekR2 = CMs(5); 

LeP =CMs(6), Y%measured from left end 


% MO] about center of mass 
10 =Is(1). 
I]. 1 =1s(2); 
IL2 = Is(3); 


IR] = Is(4); 
IR2 = Is(5), 
IP =Is(6); 


% Initial and final Jocations of third link 

% Point Q is at Node 3 (joint between Links 2 & 3) 
% Point P is at Node 4 (joint between Links 3 & 4) 
Qx0 = BoundC(1,]); Qy0 = BoundC(1,2); 

PxO = BoundC(2,]); Py0O = BoundC(?2,2), 

Qxf = BoundC(3,1); Qyf = BoundC(3,2), 

Pxf = BoundC(4,1);  Pyf = BoundC(4,2), 


ISI 


% Arms mount locations wrt spacecraft centerbody coordinate frame (rad) 
ThLO = BoundC(5,1),; ThRO = BoundC(5,2). 


% Reference Maneuver Start and Stop Times 
TO = BoundC(6,1); Tf = BoundC(6,2); 


% Constraints Matnx Flag 
AMatFlag = BoundC(8,1), 


% Centerbody Reaction Wheel Flag 
WheelF lag = BoundC(8,2), 


% Centerbody Initial and Final Conditions 
ThOO = BoundC(9,1); 
ThOf = BoundC(9,2), 


% Number of equations in the cost function constraint equations 
EOMF lag = BoundC(10,1); 


% Psuedo-Inverse Flag 
PInvFlag = BoundC(10,2), 


NMMWMVMUVVVM%MMMNVVMMUMMMMV%%M% 

%% PRELIMINARY CALCULATIONS %% 
MMV %M%%%%%%0%% 

R2D = 180/pi1, % Conversion from radians to degrees 


% Total rotation of Payload 

ThPO = atan2(Py0-Qy0,Px0-Qx0);  % Initial angle of Payload (rad) 

ThPf = atan2(Pyf-Qyf,Pxf-Qxf); % Final angle of Payload (rad) 
DelThP = ThPf - ThPO; % Total delta angle of Payload (rad) 


% Initial and final locations of Payload center of mass 
XPO = Qx0 + (PxO - QxO) * (LeP/LP), 

YPO = Qy0 + (PyO - Qy0) * (LeP/LP); 

XPf = Qxf + (Pxf - Oxf) * (LeP/LP); 

YPf = Qyf + (Pyf - Qyf) * (LcP/LP); 


Tau = (T-TO) / (Tf-TO); % Normalize time 


% Function Weighting Factors for how the payload will move 

% These factors will cause the velocity and acccleration of 

% the payload coordinates to be zero at t= 0 and t = tf. 

% They also permit the displacements for the payload coordinates 
% to match their initial and final values. These weighting 

% factors will also apply to the centcrbody rotation. 


k = Iength(Coef); 

for n=1:k 
CTau(k+I-n) = Coef(k+I -n)*Tau*(n+2); 
CTaud(k+I-n) = Cocf(k+I-n)*Tau“(n+1),; 
CTaudd(k+l1 -n) = Cocf(k+1-n)*Tau’(n); 

cnd 

% Weighting factors 

W =ConstMat(I,:)*CTau'; 

Wd = ConstMat(2,:)*CTaud’, 

Wdd = ConstMat(3,:)*CTaudd’; 


% Centerbody angle, angular velocity, angular accelcration 
DelThO = Thof - Thoo, 
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ThO = ThOO + W * Del Tho; % Angle (rad). 

ThOd = Wd * DelThoO / (Tf - TO); % Velocity (rad/see), 
ThOdd = Wdd * DelThO / (Tf - T0)*2; % Acceleration (rad/see’%2), 
*WThoO = O: 

%ThOd = 0; 

%ThOdd = 0; 

% Save for plotting 

QRef(l) = Th0, 

Cooter l) = Thod; 

QddotRef(1) = Th0dd; 


% Payload angle, angular velocity, angular acceleration 

Th = ThPO + W * DelThP; % Angle (rad) 

ThPd = Wd * DelThP / (Tf - TO); % Velocity (rad/sec) 
ThPdd = Wdd * DelThP / (Tf - T0)*2; % Acceleration (rad/sec%2) 
% Save for plotting 

QRef(6) = ThP: 

QdotRef(6) = ThPd; 

QddotRef(6) = ThPdd; 


% Payload eenter of mass position, velocity, and acceleration 
Xe = XPO + W * CXPf - XPO); 

Xed = Wd * (XPf - XPO)/ (Tf - TO); 
Xcdd = Wdd * (XPf - XPO) / (TF - T0)%2; 
Ye = YPO+ W * (YPf - YPO), 

Yed = Wd * (YPf - YPO) / (Tf - TO), 
Yedd = Wdd * (YPf - YPO)/ (Tf - TO)“2; 
% Save for plotting 

QRef(7) = Xe; 

QdotRef(7) = Xed; 

QddotRef(7) = Xedd; 

QRef(8) = Ye; 

QdotRef(8) = Yed; 

QddotRef(8) = Yedd; 


% Payload endpoint coordinates: Qx, Qy, Px, Py 
Qx = Xe - LeP * eos(ThP); 

Qy = Ye - LeP * sin(ThP), 

Px = Xe + (LP - LeP) * cos(Thl’); 

Fe yo + (LP -LceP) * sin(ThP), 


MIMD VAMVMVMMMMMMMMMMMMMMM%N% 
%% Solve for Arm Angles Required by desired path %% 
MDMMMMVMVVMVAMMVMMMMMNMMMM MN 
MMMM %MMM%%V% 
%% LEFT ARM %% 
MWiM%V%M%%%%%Y% 
% Elbow 1s left of line from arm base to Q (RQ) 
LSx = LO * eos(ThO + ThLO); 
LSy = LO * sin(ThO + ThLO); 
RQ = sqrt((Qx-LSx)*2+(Qy-LSy)*2);  % Length from arm base to Q 
Betal = atan2(Qy-LSy,Qx-LSx); % Angle from arm base to RQ 
% Law of cosines: eos(A) = (b%2 + e%2 - a%2)/(2be) 
% Apply to find angle between RQ and Link L] 
og — L172 +RO*2 - L2*%2; 
Den — 2 *L1 * RO; 
Beta2 = acos(Num/Den); % Angle from RQ to Link | 
ThL 1 = (Betal + Beta2) - (ThO + ThLO),; % Theta L1 
% Use law of cosines to find the interior angle at the elbow 
Num =L 142 + L242 - RQ“2; 
Den =2* LL! * L2: 
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Heta3 = aeos(Num/Den); 
ThL2 = -(pi-Bcta3), 
vasieel for plotting 
ORef(2) = Th 1; 
QRef(3) = ThL2; 


NMMMVV7%%%% 

%% RIGHT ARM %% 

MMMM %MM%M% 

% Elbow is nght of line from arm base (shoulder) to P (wrist) (RP) 
RSx = RO * cos(ThO + ThRO); 

RSy = RO * sin(ThO + ThRO), 

RP = sqri((Px-RSx)*2+(Py-RSy)*2); % Length from arm base to P 
Betal = atan2(Py-RSy,Px-RSx); % Angle from arm hase to RP 
% |.aw of cosines: cos(A) = (b%2 + c%2 - a%2)/(2bc) 

% Apply to find angle betwcen RP and Link R1 

Nan =1142 42 = Ro: 

Den =2 * Ri RP. 

Beta2 = acos(Num/Den); % Angle from Link R1 to RP 
Beta4 = Betal - (ThO + ThRO): 

ThR1] = -(Beta2 - Beta4); 

Num = R142 + R2%2 - RP*2; 

Den S24 RA Re. 

Beta3 = acos(Num/Den); 

ThR2 = pi - Beta3; 

% Save for plotting 

QRcef(4) = ThR1I; 

QRef(5) = ThR2; 


MMMM %%%Y%% 
%% Solve for Arm Angle Rates & Accelerations required by desired path %% 
YWRWUVUVVVVUV%VVVMVUVVHV%MV%VUV%VMUVM%MV%V%VV%MU%NMMV%%%YO% 

NMA %%%%%%% 

%% LEFT ARM %% 

WMV 0%%%% 

% [Qxd; Qyd] = [H1]*ThOd + [H2]*Thd 

% Qxd & Qyd are x & y components of point Q inertial velocity. 

% Thd = [ThL Idot; ThL2dot] 

% H matrices are made from expressing the x & v eomponents of Q in 

% terms of LO, ThO, ThLO, L1, ThL1, 12, and Thl.2. 

% Qx=LO0*eos(ThO+ThLO)+L 1 *cos(ThO+ThLO+ThL 1)+L2*eos(Tho+-... 

% ThLO+ThL 1+ThL2) 

% Qy=L0*sin(ThO+ThLO)+L 1 *sin(ThO+ThLO+ThL 1 )+L 2*sin(Tho+... 

% ThLO+ThL 1+ThL2) 

% The differentiation of thesc equations lead to 

% [Qxd; Qyd] = [H1]]*ThOd + [H2]*Thd whieh ean be solved for Thd 

Qxd = Xed + LeP * ThPd * sin(ThP), 

Qyd = Ycd - LcP * ThPd * cos(ThP); 

H2(1,2) = -L2*sin(ThO+ThLO+ThL1I+ThL 2), 

1{2(1,1) = H2(1,2) - L1*sin(ThO+ThLO+ThL 1); 

22,2) = E2*eos(ihO+ThLloO+ thi ihe2): 

H2Q,1) = H2(2,2) + El *cas(hGHinkos Thin); 

H1(1,1) = H2d,1) - LO*sin(ThO+ThLo); 

H1(2,1) = H2(2,1) + LO*cos(ThO+ThLO), 

Thd = inv(H2) * ({Qxd; Qyd] - HI *ThOd); 

% Angle rates 

ThL Id = Thd(1); 

ThL2d = Thd(2); 

% Save for plotting 

QdotRef(2) = ThL 1d; 

QdotRef(3) = ThL2d; 


% Differentiation of [Qxd; Qyd] = [FE1]*ThOd + 112]*Thd Icads to 

% [Qxdd. Qydd] = [H1dot}]*ThOd+{H 1]*ThOdd + [1 12dot]*Thd+{]12]*Thdd 
Qxdd = Xedd + LeP*(ThPdd*sin(ThP) + ThPd%2*cos(ThP)), 

Qydd = Yedd - LeP*(ThPdd*cos(ThP) - ThPd*2* sin(vhP)); 

}H12dot (1,2) = -L2*(ThOd+ThL 1d+ThL 2d)*cos(ThO+ThLO+ThL 14+ThL 2), 
112dot(1,1) = H2dot(1,2) - L1*(ThOd+ThL 1d)*cos(ThO+ThLO+ThL. 1). 
112dot(2,2) = -L2*(ThOd+ThL | d+ThL2d)*sin(Th0+ThlLO+ThL 14ThL 2); 
1 12dot(2,1) = H2dot(2,2) - L1*(ThOd+ThL 1 d)*sin(ThO+ThLO+ThL 1), 
HIdot(1,1) = H2dot 1,1) - LO*ThOd*cos(ThO+ThL.0); 

HIdot(2,1) = H2dot(2,1) - LO*ThOd* sin(ThO+Th1L0O), 

Thdd = inv(H12)*((Qxdd;, Qydd]-H2dot* Thd-[1]1dot]*ThOd- (111 ]*ThOdd): 
% Angle accelerations 

ThL Idd = Thdd(1), 

ThE 2dd = Thdd(2); 

QddotRef(2) = ThL Idd; 

QddotRef(3) = ThL 2dd, 


MWUWWDWVVV%V%%N%Y%o 

%% RIGHT ARM %% 
WAVVVVVUVNI%V%% 

% The devclopment is similar to the Icft arm 

% Px=RO*cos(ThO+ThRO)+R 1] *cos(ThO+ThRO+ThiR1)+R2*cos(ThO+... 
% ThRO+ThR1+ThR2) 

% Py=RO*sin(ThO+ThRO)+R 1 *sin(ThO+ThRO+ThHR1)+R 2* sin(ThO+... 
% ThRO+ThR1+ThR2) 

% [Pxd; Pyd] = [H1 ]*ThOd + [H2)*Thd 

Pxd = Xcd - (LP - LeP) * ThPd * sin(ThP), 

Iva = Yod + (LP -LceP) * ThPd * cos(ThP), 
112(1,2) = -R2*sin(ThO+ThRO+ThR 1+ThR2): 
112¢1,1) = H2(1,2) - R1*sin(ThO+ThRO+ThR 1); 
142(2,2) = R2*cos(ThO+ThRO+ThR1+ThR2), 
22,1) = H2(2,2) + R1*cos(ThO+ThRO+ThR 1), 
H1¢1,1) = H2c1,1) - RO*sin(ThO+ThRO). 
H1(2,1) = H2(2,1) + RO*cos(ThO+ThRO), 

Thd = inv(H2) * ({[Pxd, Pyd] - H1*Th0Od); 

“% Angle rates 

ThR1d = Thd(1), 

ThR2d = Thd(2); 

% Save for plotting 

QdotRef(4) = ThR 1d; 

QdotRef(5) = ThR2d, 


% [Pxdd; Pydd] = [Ht dot]*ThOd+[1]11]*ThOdd + [H2dot]*Thd+[H2]*Thdd 
Pxdd = Xcdd - (LP - LeP)*(ThPdd*sin(ThP) + ThPd*2*cos(ThP)), 
yad = Ycdd + (LP - LeP)*(ThPdd*cos(ThP) - ThPd*2*sin(ThP)): 
H2dot(1,2) = -R2*(ThOd+ThR 1d+ThR 2d)*cos(Th0+ThRO+ThR1+ThR2); 
H2dot(1,1) = H2dot(1,2) - R1*(ThOd+ThR 1d)*cos(Th0+ThRO+ThR 1); 
H2dot(2,2) = -R2*(ThOd+ThR 1d+ThR2d)*sin(Th0O+ThRO+ThR 1+ThR2), 
H2dot(2,1) = H2dot(2,2) - R1*(ThOd+ThR 1d)*sin(ThO+ThRO+ThR 1), 
H1dot(1,1) = H2dot(1,1) - RO*ThOd*cos(ThO+ThRO); 

H1dot(2,1) = H2dot(2,1) - RO*ThOd*sin(Th0O+ThRO); 

Thdd = inv(H2)*([Pxdd; Py dd]-H2dot* Thd-[H | dot]* ThOd-[FH1]*ThOdd), 
% Angle accelerations 

ThR1dd = Thdd(1); 

ThR2dd = Thdd(2); 

QddotRef(4) = ThR 1dd; 

QddotRef(5) = ThR2dd, 


WWMVWV%UUVWVWVVWVUNVVVVYY% 
%% Find necded control torqucs, u %% 


ls 


MMMM NMMMAMAMVM%M%%M%Y% 
% EOM: M*qddot + G = B*u + A'*Lam 
% Constraint Eqns: A*qdot = 0 
% Solve EOM for qddot leads to 
%  qddot = inv(M)*(B*u + A'*Lam - G) 
% Differentiate Constraint Eqns gives Adot*qdot + A*qddot = 0 
% Substitute qddot denved from EOM into differentiated constraint 
% eqns and solve for Lam 
% Lam = -inv(A*inv(M)*A‘')*(A *inv(M)*(B*u-G)+A dot*qdot) 
% Substitute this expression for Lam into origian] EOM. Collect terms 
% into the form MTilda*qddot + GTilda = BTilda*u 
% where MTilda = M 
% GTilda = G + A inv(A *inv(M)*A')*(Adot* qdot-A *inv(M)*G) 
% BTilda =(1-A'*inv(A*inv(M)*A‘')*A *inv(M))*B 
% The first five resulting equations apply to the spacecraft centerbody 
% and arms. The final three apply to the payload. The matrix form of 
% the last three equations 1s 
% MPTilda*QPddot + GPTilda = BPTilda*u 


MWWVAVM*wMVMMN% 
%% Matrices %% 
YWUVMVVMV%% 
AngConst = [ThLO; ThRO}, 
%AMatF lag = 1; 
if AMatFlag 
|M,G,A,Adot,B] = MatxFix(Ls,Ms,CMs,Is,QRef,QdotRef,AngConst); 
else 
{M,G,A,Adot,B] = Matx(Ls,Ms,CMs, Is,QRef, QdotRef,AngConst); 
end 


if WheelF lag 
B7 = [1; 0; 0; 0; 0; 0. 0; OJ; 
B = [B7 B}. 

end 


% If the cost function is subject to the constraint that the payload 

% satisfy the reference motion, then three equations of motion are used. 
% To include the centerbody reference motion, usc four equations from 
% the equations of motion. 


WAVY 

%% MTilda %% 

WMV %%% 

if LOMFlag == 3 % Use only the payload equations 
MPTilda = M(6:8,6:8); 

else 
if EOMFlag==5 % Use the spacecraft equations 

MPTilda = M(1:5,1:5); 


else % Use all eight equations 
MPTilda = M; 
end 
end 


WWMM UWVV%M% 
%% GTilda %% 
WV VVV%%Y% 
Qdot = QdotRef; 
GTilda = G + A'*inv(A *inv(M)*A')*(Adot*Qdot - A*inv(M)*G); 
if EOMFlag == 3 % Use only the payload equations 
GPTilda = GTilda(6:8, 1); 
else 
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if EOMFlag == % Use the spacecraft equations 
GPTilda = GTilda(1:5,1); 


else % Use all eight equations 
GPTilda = GTilda: 
end 
end 


MMMM M%N% 

%% BTilda %% 

WMV %%% 

13 Tilda = (eye(8) - A'*inv(A *inv(M)*A')*A *inv(M))*B, 


if EOMFlag == % Use only the payload equations 
BPTilda = BTilda(6:8,:); 
else 
if EOMF lag == % Use the spaceeraft equations 
BPTilda = BTilda(1:5,:); 
else % Use all eight equations 
BPTilda = BTilda; 
end 
end 


MWUwMMM%V%%% 

M% GI & G2 %% 

MWVAVDV%%%Y% 

% Use previous expression for Lam and regroup terms into the following 
%form A'*Lam=K1+K2*u 

K 1 = A'*inv(A *inv(M)*A‘')*(A *inv(M)*G-Adot* Qdot); 

K2 = -A'*inv(A*inv(M)*A')*A *inv(M)*B; 


HMMWV %MM%% 

%% Torques %% 

MMMM %M%% 

% Torques are calculated to minimize the following cost funetion: 

vo J —0.5*[(u*Wut*u + Lam'*A*We*A'*Lam + Jr*Wr* Tr] 

% Subject to the constraint: MP*QPddot + GPTilda - BPTilda*u = 0 
’% Combine the constraint into the cost funetion by multiplying the 

% constraint eqn by another Lagrange multiplier, Gam, and adding that 
% to the cost function. Take the gradient with respeet to u results in 

% (WutK2'*We*K2)*u + K2'*We*K] - BPTilda’*Gam = 0 

% Solve for u 

% u=inv(WutkK2'*We*K2)*(BPTilda*Gam - K2'*We*K 1) 

% Substitute this into the constraint eqn. Solve the result for Gam 

% Gam = inv(BPTilda*inv(Wu + K2'*We*K2)*BPTilda’)*(MP*QPddot+ 
% GPTilda+BP Tilda*inv(Wu + K2'*We*K2)*K2'*We*K 1) 
% Substitute this expression into the torque equation, u. 


Qddot = QddotRef; 


if EOMF lag == 3 % Use only the payload equations 
QPddot = Qddot(6:8,1); 


else 
if EOMF lag == % Use the spacecraft equations 
QPddot = Qddot(1:5,1); 
else % Use all eight equations 
QPddot = Qddot, 
end 
end 


YHWWVWVVVVV%W%%%%% 
%% PSUEDO-INVERSES %% 
YRWWWWVVWWVWV% WY 


les 


% To avoid the problems with poorly conditioned matnees, I've used the 
% psucdo-inverse rather than the traditional inverse in the next two 
% equations. 
if PInvilag 
Part] = pinv(Wu + K2'*We*K2); 
Gam = pinv(BPTilda* Part] *BPTilda’) * (MPTilda*QPddot + GPTilda +... 
BPTilda* Part 1 *(K2'*We*K1)); 
else 
Part] = inv(Wu + K2'* We*K2), 
Gam = inv(BPTilda*Part 1 *BPTilda') * (MPTilda*QPddot + GPTilda +... 
BPTilda* Part] *(K2'*Wc*K 1)); 
end 


% Reference Torques 
Torques = Part] *(BPTilda’*Gam - K2'*We*K 1). 


% Cost Funetion, J 
J = abs(Torques(1)); 


% Controller Info 

Pt! = A'™*inv(A *inv(M )*A‘); 

C] = inv(M)*(eye(M) - Pt] *A*inv(M))*B; 
C2 = -inv(M)* Pt] * Adot; 

C3 = inv(M)*(Pt] *A*inv(M) - eye(M))*G; 


MWUVWADVV%VV%%%% 

%% DEBUG INFO %% 

MAMAN %Y% 

%% Are constraint equations, A*qdot=0, satisfied? 
Aqdot = A*QdotRef, 


. RefMin2 


% Filename is 'RefMin2.m' 

% Reference Maneuver using cost funetion 

% This routine 1s used by "MainOpt.m" to find the optimal combination 
% of reference trajectory polynomial coefficients. 

% Version 2 uses the rate of change of angular momentum to find 

% the wheel torque. 


funetion [Jopt] Jopt2] = RefMin2(T,Ls,Ms,CMs,Is, BoundC, Wu, We,Coef,ConstMat) 


% OUTPUTS: 
% Jopt = absolute value of the reaetion wheel torque. This 1s the cost 


% function value for purposes of optimizing the reference 
% trajectory polynomial coefficients. Jopt] will be integrated by 
% odemin.m while Jopt2 is the same value but won't be integrated. 
0 

0 
% INPUTS: 


% T = time (sec) 
% Ls = 7x1 veetor of lengths (m) 


% Ist element = distance from origin to left arm mount 

% 2nd & 3rd elements wrt left arm (from shoulder toward wrist) 
% 4th element = payload length 

% Sth & 6th elements wrt nght arm (from wnst toward shoulder) 
% 7th element = distance from nght arm mount to ongin 


% [EOS E2 Ps R2 Ri RO] 
% Ms = 6x1] column vector containing the masses (kg) 
% Ist element = mass of spacecraft eenterbody 
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% 2nd & 3rd elements = mass of left arm (upper arm then lower arm) 
% 4th & 5th clements = mass of right arm (upper arm then lower arm) 
%, 6th element = payload mass 

% [MO; ML1; ML2; MRI; MR2; MP] 

% CMs = 6x1 column vector containing center of mass locations 

% (ecOsbclel eck itek ly Leck 2. EcP} 

% Is = 6x1 column vector containing the moments of mnertias about the 


% respective body's center of mass (kg m%2) 

% Ist element = inertia of spacecraft eenterbody 

% 2nd & 3rd elements = incrtia of left arm (upper arm then lower arm) 
% Ath & Sth elements = inertia of mght arm (upper arm then lower arm) 


% 6th element = payload incrtia 

% TO Te kA 2: iP} 

% BoundC = boundry conditions for the problem. The first column 
% contains the initial x and y component of points Q & P 

% respectively, the x component of the right arm base. the 

% problem start time, and the simulation stop time. The second 
% column contains the x and y component of points Q & P 

% respectively, the x component of the nght arm base, the 

% stop time for the ideal reference maneuver, and a flag to 

% activate or deactivate the controller. The ongin for the 

% x andy components is the base of the left arm. 

% Wu = 6x6 control torque cost weighting matrix 

% We = 8x8 constraint cost weighting matrix 

% Coef = (n-2)x1 vector of polynomial refcrence trajcctory coefficients 


% in descending order where n 1s the highest order coefficient 
% ConstMat = 3x(n-2) matrix of coefficients for reference trajectory 
% displacement (row 1), velocity (row 2) and acceleration (row 3) 


ARVVVMMMMMAMMMMMMMMMMMMMMMMMMMMYN 
%% CONVERT INPUTS FROM ARRAYS TO SCALARS %% 
WWMM MMMM %% 
% Lengths (m) 

LO = Ls(1): 

LI] =Ls(2); 

.2 = Ls(3); 

LP =Ls(4), 

R2 =Ls(S), 

R] =Ls(6), 

RO = Ls(7), 


“% Member masses (kg) 
MO = Ms(1); 

ML1 = Ms(2); 

ML2 = Ms(3); 

MR] = Ms(4); 

MR2 = Ms(5), 

MP = Ms(6); 


% Center of mass distances (m) 

LcO =CMs(1); 

LeL] = CMs(2), 

LcL2 =CMs(3); 

LcR1 = CMs(4), 

LcR2 = CMs(5), 

LcP =CMs(6), %measured from left end 


% MOI about center of mass 


10 = Is(1); 
IL} =Is(2); 
IL2 = Is(3), 


| SN, 


IRI = Is¢4), 
IR2 = Is(5); 
IP = Is(6); 


% Initial and final locations of third link 

% Point Q is at Node 3 (joint between Links 2 & 3) 
% Point P 1s at Node 4 (joint between Links 3 & 4) 
Qx0 = BoundC(1,}); QyO = BoundC(1,2); 

PxO = BoundC(2,1); =Py0 = BoundC (2,2); 

Oxf = BoundC(3,1);  Qyf = BoundC(3,2); 

Pxf = BoundC(4,]); = Pyf = BoundC¢4,2)- 


% Arms mount locations wrt spacccraft centerbody coordinate frame (rad) 
Thl.0 = BoundC(5,1); ThRO = BoundC(5,2), 


% Refcrence Maneuver Start and Stop Times 
TO =BoundC(6,1); Tf = BoundC(6,2); 


% Constraints Matnx Flag 
AMat?lag = BoundC(8,1); 


% Centerbudy Reaction Wheel Flag 
Wheel! lag = BoundC(8,2); 


% Centcrbody Initial and Final Conditions 
ThOO = BoundC(9,] ); 
ThOf = BoundC(9,2); 


% Number of equations in the cost function constraint equations 
EOMF lag = BoundC(10,1); 


% Psuedo-Inverse Flag 
PinvFlag = BoundC(10,2); 


MAMP~AAMWVVAVMVVLMVMUVVV%%N% 

%% PRELIMINARY CALCULATIONS %% 
MVMMVADVVAVAVAVMV/0%VVM%VO% 

R2D = 180/pi; + % Conversion from radians to degrces 


% Total rotation of Payload 

ThPO = atan2(Py0-Qy0,Px0-Qx0); = % Initial angle of Payload (rad) 

ThPf = atan2(Pyf-Qy f,Pxf-Qxf), % Final angle of Payload (rad) 

Del The hei ae % Total delta angle of Payload (rad) 


% Initial and final locations of Payload centcr of mass 
XP?O = QxO + (Px0 - Qx0) * (LeP/LP), 

YPO = Qy0O + (PyO - Qy0) * (LcP/LP); 

XPf = Oxf + (Pxf - Qxf) * (LeP/LP),; 

WETS Ovign (yi Ovi) me teil 


Tau = (T-TO) / (Tf-TO), % Normalize time 


% Function Weighting Factors for how the payload will move 
% These factors will cause the angular velocity and angular 
% acceleration of the payload to be zero at t = 0 and t = tf 

% They also permit the payload angle to match its initial 

% and final values. These weighting factors will also apply 

% to the translational motion of the payload center of mass. 


k = length(Coef), 
for n=I]:k 
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CYau(k+1-n) = Coef(k+1-n)* Tau%(n+2),; 
CTaud(k+}-n) = Coef(k+1-n)*Tau%(n+1); 
CTaudd(k+1-n) = Coef(k+I-n)*Tau“(n); 

end 

W =ConstMat(1,:)*CTau'; 

Wd = ConstMat(2,:)*CTaud’; 

Wdd = ConstMat(3,:)*C Taudd’, 


% Centerbody angle, angular velocity, angular aeeeleration 
DelThO = ThOf - Thoo; 

ThO = ThOO + W * Del Tho; % Angle (rad); 

ThOd = Wd *¥ DelThO / (Tf - TO); % Velocity (rad/see), 

ThOdd = Wdd * Del ThO / (Tf - TO)*2; % Acceleration (rad/sce’%2); 
% Save for plotting 

QRef(l) = Tho; 

QdotRef(1) = ThOd; 

QddotRef(1) = ThOdd; 


% Payload angle, angular velocity, angular acceleration 

ie thnPO+ W * DelThl’, % Angle (rad) 

ThPd = Wd * DelThP / (Tf - TO), % Veloeity (rad/sec) 
ThPdd = Wdd * DelThP / (Tf - T0)*2; % Acceleration (rad/see*2) 
% Save for plotting 

QRef(6) = ThP; 

QdotRef(6) = ThPd; 

QddotRef(6) = ThPdd; 


% Payload center of mass position, velocity, and aceeleration 
Xe = XPO + W * (XPf - XPO); 

Xed = Wd * (XPf - XPO) / (Tf - TO); 
Xedd = Wdd * (XPf - XPO) / (Tf - TO)*2; 
he YPO+ W *(YPf - YPO); 

Yed = Wd * (YPf - YPO)/ (Tf - TQ), 
Yedd = Wdd * (YPf - YPO) / (Tf - TO)%2; 
% Save for plotting 

QRef(7) = Xe; 

QdotRef(7) = Xed; 

QddotRef(7) = Xedd; 

Gikei(8) = Ye: 

QdotRef(8) = Yed: 

QddotRef(8) = Yedd; 


% Payload endpoint eoordinates: Qx, Qy, Px, Py 
Qx = Xe - LeP * eos(ThP); 

Qy = Ye - LeP * sin(ThP), 

Pee xc + (LP -LeP) * cos(ThP); 

Pye yc + (LP -LeP) * sin(ThP); 


MMMM’ MLMMMMMMUMAMUMMMYV%%% 
%% Solve for Arm Angles Required by desired path %% 
MAMMA MAMMVMMMMMMMMMMMWAY 
MAMMA AY 
%% LEFT ARM %% 
MAVMVAMMNM%% 
% Elbow ts left of line from arm base to Q (RQ) 
LSx = LO * eos(ThO + ThLO); 
LSy = LO * sin(ThO + ThLO); 
RQ = sqrt((Qx-LSx)*2+(Qy-LSy)*2); = % Length from arm base to Q 
Betal = atan2(Qy-LSy,Qx-LSx); % Angle from arm base to RQ 
% Law of cosines: cos(A) = (b%2 + e%2 - a%2)/(2bc) 
% Apply to find angle between RQ and Link L 1 


16] 


Num= 72 FRO 2. 

Den =2 * LI * RQ; 

Beta2 = acos(Num/Den), % Angle from RQ to Link | 
ThI.1 = (Betal + Beta2) - (ThO + ThLO); % Theta I] 

% Use law of cosines to find the interior angle at the clbow 
Num =L1%2 +£2°2= KO 2, 

Den =2 * L1 * L2; 

Beta3 = acos(Num/Den), 

Thl.2 = -(pi-Beta3); 

%Save for plotting 

QRef(2) = ThL I; 

QRef(3) = ThL 2, 


WAWVVVAAWV%V%V% 

%% RIGHT ARM %% 

MMAVUWAMMM%%%% 

% Elbow is nght of line from arm base (shoulder) to P (wrist) (RP) 
RSx = RO * cos(ThO + ThRO); 

RSy = RO * sin(ThO + ThRO); 

RP = sqrt((Px-RSx)*2+(Py-RSy)*2), % Length from arm base to P 
Betal = atan2(Py-RSy,Px-RSx), = _% Angle from arm base to RP 
% |.aw of cosines: cos(A) = (b*2 + c%2 - a*2)/(2bc) 

% Apply to find angle between RP and Link RI 

Num = R142 + RP%2 - R2%2; 

Den =2 * RI * RP; 

Beta2 = acos(Num/Den); % Angle from Link R1 to RP 
Beta4 = Betal - (ThO + ThRO), 

ThRI = -(Beta2 - Beta4); 

Num = R142 + R2%2 - RP*%2; 

Den =2 * RI * R2; 

Beta3 = acos(Num/Den), 

ThR2 = pt - Beta3, 

% Save for plotting 

QRef(4) = ThRI. 

QRel(5) = ThR2; 


MMVWVAVAVAVANMMM%%%MMMUVM%%% 
%% Solve for Arm Angle Rates & Accelerations %% 
%% required by desired path M% 
WAMAWMVVAMM%MMMMMVMU%MM%%YOM% 
MMUVVMVMV%%%0%Y% 
%% LEFT ARM %% 
MMP MMV%Y% 
% [Qxd, Qyd] = [HI ]*ThOd + [H2]*Thd 
% Qxd & Qyd arc x & y componcnts of point Q inertial velocity. 
% Thd = [ThLIdot,; ThL2dot] 
% H matrices are made from expressing the x & y components of Q 1n 
% tcrms of LO, ThO, ThLO, L1, ThL1, L2, and Thh2. 
% Qx=LO*cos(ThO+ThLO)+L | *cos(ThO+Thl.0+ThL 1)+L2*cos(Tho-+-... 
ThLO+ThL 1+ThL2) 
% Qy=LO0*sin(ThO+ThLO)+L I *sin(ThO+ThLO+ThL J )+L2*sin(Tho+... 
ThLO+ThL1I+ThL2) 
% The differentiation of these equations lead to 
% [Qxd: Qyd] = [H1]]*ThOd + [H2]*Thd which can be solved for Thd 
Qxd = Xced + LcP * ThPd * sin(ThP); 
Oyd = Yed -Ver* thPd * cosh). 
112(1,2) = -L.2*sin(ThO+ThLO+ThL 1+Th1 2); 
Fl2¢1,1) = H2¢1,2) - L1*sin(ThO+ThLO+Th_ 1); 
F2(2,2) = L2*cosCho+ Thos ile ane?) 
H2(2,1) = H2(2,2) + LI *cos(Tho+ThlLo+Thk me 
HICI,1) = H2¢1,1) - LO*sin(Th0+ThLO); 
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2. ble (21) + 1 0*cos( ThOF Th), 
Thd = inv(112) * ({Qxd; Qyd] - H1*ThOd), 
% Angle rates 

ThL Id = Thd(1); 

ThL 2d = Thd(2), 

% Save for plotting 

QdotRef(2) = Th 1d; 

QdothRef(3) = Th. 2d; 


% Differentiation of [(Qxd; Qyd] = [H1]*ThOd + [112]*Thd Icads to 

% {Qxdd; Qydd] = [H1}dot]*ThOd+[H1]*ThOdd + [[]2dot]* Thd+[112|*Thdd 
Qndd = Xedd + LeP*(ThPdd*sin(ThP) + ThPd*2*cos(ThP)); 

Qydd = Yedd - LeP*(ThPdd*cos(ThP) - ThPd*2*sin(ThP)), 

H2dot(1,2) = -L2*(ThOd+ThL 1d+ThL 2d)*cos(ThO+ThI_0O+ThL 14+ThL 2), 
H2dot(1,1) = H2dot(1,2) - L1*(ThOd+ThL 1d)*cos(ThO+ThL_O+ThI. 1); 
I12dot(2,2) = -L2*(ThOd+ThL 1d+ThL 2d)*sin(ThO+ThLO+ThL 1+ThL 2); 
H2dot(2,1) = H2dot(2,2) - L1*(ThOd+ThL 1d)*sin(ThO+ThhLO+Th_ 1); 
Hidot(1,!) = H2dot(1,1) - LO*ThOd*cos(Th0+ThL0),; 

I} dot(2,1) = H2dot(2,1) - LO*ThOd* sin(ThO+ThLO), 

Thdd = inv(H2)*({Qxdd; Qydd]-I12dot* Thd-{H 1 dot]*ThOd-[F1 1 ]*ThOdd); 
% Angle accelcrations 

ThL Idd = Thdd(1),; 

ThL2dd = Thdd(2), 

QddotRef(2) = ThL Idd; 

QddotRef(3) = ThL 2dd; 


MMMM%%MM%%%% 

%% RIGHT ARM %% 

MWM%MV%V%%%%%M% 

% The development is similar to the left arm 

% Px=RO*cos(ThO+ThRO)+R 1 *cos(ThO+ThRO+ThR 1 )+R2*cos(Tho-+... 
ThRO+ThR1+ThR2) 

% Py=RO*sin(ThO+ThRO)+R I *sin(ThO+ThRO+ThR 1)+R2*sin(ThO+... 
ThRO+ThR1+ThR2) 

% [Pxd, Pyd] = [H1]*ThOd + [H2]*Thd 

Pxd = Xcd - (LP - LcP) * ThPd * sin(ThP), 

Pua = Ycd +(LP-LcP) * ThPd * cos(ThP): 

H2(1,2) = -R2*sin(ThO+ThRO+ThR1+ThR2), 

H2(1,1) = H2(1,2) - RI *sin(ThO+ThRO+ThR 1); 

112(2,2) = R2*cos(ThO+ThRO+ThR1!+ThR2),; 

H2(2,1) = H2(2,2) + RI *cos(ThO+ThRO+ThR 1); 

HIi¢t,1) = H2 (1,1) - RO*sin(ThO+ThRO), 

H1(2,1) = H2(2,1) + RO*cos(ThO+ThRO), 

Thd = inv(H2) * ({Pxd; Pyd] - HI*ThOd); 

% Angle rates 

ThR Id = Thd(1); 

ThR2d = Thd(2), 

% Save for plotting 

QdotRef(4) = ThR1Id; 

QdotRef(5) = ThR2d; 


% [Pxdd;, Pydd] = [H1dot}*ThOd+[H1 ]*ThOdd + [1 ]2dot]*Thd+[112]*Thdd 
Pxdd = Xcedd - (LP - LeP)*(ThPdd*sin(ThP) + ThPd’2*cos(ThP)); 

Pydd = Yedd + (LP - LeP)*(ThPdd*cos(ThP) - ThPd*2*sin(ThP)); 
H2dot(1,2) = -R2*(ThOd+ThR 1d+ThR2d)*cos(ThO+ThRO+ThR1+ThR2); 
H2dot(1,1) = H2dot(1,2) - R1*(ThOd+ThR 1d)*cos(ThO+ThRO+ThR 1): 
H12dot(2,2) = -R2*(ThOd+ThR 1d+ThR 2d)* sin(ThO+ThRO+ThR 1+ThR2); 
112dot(2,1) = H2dot(2,2) - R1*(ThOd+ThR 1d)*sin(ThO+ThRO+ThR 1); 

Hi dot(1,1) = H2dot(1,1) - RO*ThOd*cos(ThO+ThRO), 

Hi dot(2,1) = H2dot(2,1) - RO*ThOd*sin(ThO+ThRO), 

Thdd = inv(H2)*(({Pxdd; Py dd]-H2dot*Thd-{H1 dot]* ThOd-[F11]*ThOdd); 
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% Angle accelerations 
ThR1dd = Thdd(1); 
ThR2dd = Thdd(2); 
QddotRef(4) = ThR 1 dd; 
QddotRef(5) = ThR2dd; 


YW—VWAAHWMADVVMVM%%MVYV%% 
%% ‘ind needed eontrol wheel torque %% 
MMVWVAMVAVVVVV%MVV%%0% 

Q =QRef: 

Qdot = QdotRef; 

Qddot = QddotRef; 


[Hs, Hdots] = AngMo2(Ls,Ms,CMs,Is,Q.Qdot,Qddot): 


% Cost Funetion, Jopt 

% Wheel torque is the change in total angular momentum 
% Joptl is integrated while Jopt 2 is not 

Joptl = abs(Hdots(7)); 

Jopt2 = Joptl,; 
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